/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "voslam_geo.h"
#include "voslam_blas.h"
#include "voslam_util.h"

/* gcc -O2 -Wall -g -c -o voslam_geo.o voslam_geo.c */
void test_chirality(double *v,double *pts2,double *pts1,int num_points, int *pos)
{
    double J[16],P[12],P1[12],V[16],U[16],S[4],X[4];
    double E[9],num,den,a1,a2,b1,b2,c1,pts11,pts12,pts21,pts22,e,mins,q[4],t[3],x1,y1,x2,y2;
    int i=0,j,k,cnt;

    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];

    quater_2_rot(P,q);
   
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    P1[0]=1.;
    P1[1]=0.;
    P1[2]=0.;
    P1[3]=0.;
    P1[4]=1.;
    P1[5]=0.;
    P1[6]=0.;
    P1[7]=0.;
    P1[8]=1.;
    P1[9]=0.;
    P1[10]=0.;
    P1[11]=0.;
    
    E[0]=t[1]*P[2]-t[2]*P[1];
    E[1]=t[2]*P[0]-t[0]*P[2];
    E[2]=t[0]*P[1]-t[1]*P[0];
    E[3]=t[1]*P[5]-t[2]*P[4];
    E[4]=t[2]*P[3]-t[0]*P[5];
    E[5]=t[0]*P[4]-t[1]*P[3];
    E[6]=t[1]*P[8]-t[2]*P[7];
    E[7]=t[2]*P[6]-t[0]*P[8];
    E[8]=t[0]*P[7]-t[1]*P[6];
    cnt=0;
    for(i=0;i<num_points;i++)
    {
        x2=pts2[i*2];
        y2=pts2[i*2+1];
        a2=E[0]*x2+E[3]*y2+E[6];    //E(1,:)*pts2(:)
        b2=E[1]*x2+E[4]*y2+E[7];    //E(2,:)*pts2(:)
        //c2=E[2]*pts2[i*2]+E[5]*pts2[i*2+1]+E[8];    //E(3,:)*pts2(:)
        x1=pts1[i*2];
        y1=pts1[i*2+1];
        a1=E[0]*x1+E[1]*y1+E[2];    //pts1(:)'*E(:,1)
        b1=E[3]*x1+E[4]*y1+E[5];    //pts1(:)'*E(:,2)
        c1=E[6]*x1+E[7]*y1+E[8];    //pts1(:)'*E(:,2)
        num=1/(a1*a1+b1*b1+a2*a2+b2*b2);
        den=a1*x2+b1*y2+c1;
        e=den*num;//(a1*a1+b1*b1+a2*a2+b2*b2);

        pts11=x1-e*a2;
        pts12=y1-e*b2;
        pts21=x2-e*a1;
        pts22=y2-e*b1;
        
        J[0] = P[2] * pts11 - P[0];
        J[4] = P[5] * pts11 - P[3];
        J[8] = P[8] * pts11 - P[6];
        J[12] = P[11] * pts11 - P[9];
    
        J[1] = P[2] * pts12 - P[1];
        J[5] = P[5] * pts12 - P[4];
        J[9] = P[8] * pts12 - P[7];
        J[13] = P[11] * pts12 - P[10];
   
        J[2] = P1[2] * pts21 - P1[0];
        J[6] = P1[5] * pts21 - P1[3];
        J[10] = P1[8] * pts21 - P1[6];
        J[14] = P1[11] * pts21 - P1[9];
        
        J[3] = P1[2] * pts22 - P1[1];
        J[7] = P1[5] * pts22 - P1[4];
        J[11] = P1[8] * pts22 - P1[7];
        J[15] = P1[11] * pts22 - P1[10];
        
        svduv(S,J,U,4,V,4);
        j=0;
        mins=S[j];

        for(k=1;k<4;k++)
        {
            if(mins>S[k])
            {
                j=k;
                mins=S[k];
            }
        }
          /* ? */
        if(fabs(U[12+j])>0.0000000001)
        {
            if(U[12]<0)
                 U[12]=-0.0000000001;
            else
                 U[12]=0.0000000001;
        }

        X[2]=U[8+j]/U[12+j];
        if(X[2]>0)
            cnt++;
    }

    (*pos)=cnt;
}
void compose_E(double *E,double *q,double *t)
{
  double R[9];
  
   quater_2_rot(R,q);
  
  E[0]=t[1]*R[2]-t[2]*R[1];
  E[1]=t[2]*R[0]-t[0]*R[2];
  E[2]=t[0]*R[1]-t[1]*R[0];
  E[3]=t[1]*R[5]-t[2]*R[4];
  E[4]=t[2]*R[3]-t[0]*R[5];
  E[5]=t[0]*R[4]-t[1]*R[3];
  E[6]=t[1]*R[8]-t[2]*R[7];
  E[7]=t[2]*R[6]-t[0]*R[8];
  E[8]=t[0]*R[7]-t[1]*R[6];
}

void compose_F(double *F,double *q,double *t,double *pl,double *pr)
{
  double R[9],E[9],K[9],iK[9];
  int i,j,k;
  
  quater_2_rot(R,q);
  
  F[0]=t[1]*R[2]-t[2]*R[1];
  F[1]=t[2]*R[0]-t[0]*R[2];
  F[2]=t[0]*R[1]-t[1]*R[0];
  F[3]=t[1]*R[5]-t[2]*R[4];
  F[4]=t[2]*R[3]-t[0]*R[5];
  F[5]=t[0]*R[4]-t[1]*R[3];
  F[6]=t[1]*R[8]-t[2]*R[7];
  F[7]=t[2]*R[6]-t[0]*R[8];
  F[8]=t[0]*R[7]-t[1]*R[6];
  
  K[0]=1./pl[0];
  K[1]=0.;
  K[2]=-pl[1]/pl[0];
  K[3]=0.;
  K[4]=1./pl[0];
  K[5]=-pl[2]/pl[0];
  K[6]=0.;
  K[7]=0.;
  K[8]=1.;
  iK[0]=1./pr[0];
  iK[1]=0.;
  iK[2]=-pr[1]/pr[0];
  iK[3]=0.;
  iK[4]=1./pr[0];
  iK[5]=-pr[2]/pr[0];
  iK[6]=0.;
  iK[7]=0.;
  iK[8]=1.;
  for(i=0;i<3;i++)
  {
      for(j=0;j<3;j++)
      {
          E[i*3+j]=K[i]*F[j*3];
          for(k=1;k<3;k++)
          {
              E[i*3+j]+=K[i+k*3]*F[k+j*3];    
          }
      }
  }
   //     printf("H=[%f %f %f %f %f %f %f %f %f]\n",H[0],H[1],H[2],H[3],H[4],H[5],H[6],H[7],H[8]);
  for(i=0;i<3;i++)
  {
      for(j=0;j<3;j++)
      {
          F[i+j*3]=E[i*3]*iK[j];
          for(k=1;k<3;k++)
          {
              F[i+j*3]+=E[i*3+k]*iK[k*3+j];    
          }
      }
  }
}

void find_zero_det_F(double *E,double *F1,double *F2)
{
  double p0,p1,a;
  int i,j,k,c,cyc=30;
  a=0.5;
  for(c=0;c<cyc;c++)
  {
      for(i=0;i<9;i++)
      {
          E[i]=a*F1[i]+(1-a)*F2[i];
      //(F2[i]+a*(F1[i]-F2[i])
      }
  
      p0=-E[6]*E[4]*E[2]-E[7]*E[5]*E[0]-E[8]*E[3]*E[1]+E[0]*E[4]*E[8]+E[1]*E[5]*E[6]+E[2]*E[3]*E[7];
  
      i=6;
      j=4;
      k=2;
      p1=-(F2[i]*F2[j]*(F1[k]-F2[k])+F2[i]*F2[k]*(F1[j]-F2[j])+F2[j]*F2[k]*(F1[i]-F2[i]))
      -2*a*(F2[i]*(F1[j]-F2[j])*(F1[k]-F2[k])+(F1[i]-F2[i])*F2[j]*(F1[k]-F2[k])+(F1[i]-F2[i])*(F1[j]-F2[j])*F2[k])
      -3*a*a*(F1[i]-F2[i])*(F1[j]-F2[j])*(F1[k]-F2[k]);
      i=7;
      j=5;
      k=0;
      p1+=-(F2[i]*F2[j]*(F1[k]-F2[k])+F2[i]*F2[k]*(F1[j]-F2[j])+F2[j]*F2[k]*(F1[i]-F2[i]))
      -2*a*(F2[i]*(F1[j]-F2[j])*(F1[k]-F2[k])+(F1[i]-F2[i])*F2[j]*(F1[k]-F2[k])+(F1[i]-F2[i])*(F1[j]-F2[j])*F2[k])
      -3*a*a*(F1[i]-F2[i])*(F1[j]-F2[j])*(F1[k]-F2[k]);
      i=8;
      j=3;
      k=1;
      p1+=-(F2[i]*F2[j]*(F1[k]-F2[k])+F2[i]*F2[k]*(F1[j]-F2[j])+F2[j]*F2[k]*(F1[i]-F2[i]))
      -2*a*(F2[i]*(F1[j]-F2[j])*(F1[k]-F2[k])+(F1[i]-F2[i])*F2[j]*(F1[k]-F2[k])+(F1[i]-F2[i])*(F1[j]-F2[j])*F2[k])
      -3*a*a*(F1[i]-F2[i])*(F1[j]-F2[j])*(F1[k]-F2[k]);
      i=0;
      j=4;
      k=8;
      p1+=-(F2[i]*F2[j]*(F1[k]-F2[k])+F2[i]*F2[k]*(F1[j]-F2[j])+F2[j]*F2[k]*(F1[i]-F2[i]))
      -2*a*(F2[i]*(F1[j]-F2[j])*(F1[k]-F2[k])+(F1[i]-F2[i])*F2[j]*(F1[k]-F2[k])+(F1[i]-F2[i])*(F1[j]-F2[j])*F2[k])
      -3*a*a*(F1[i]-F2[i])*(F1[j]-F2[j])*(F1[k]-F2[k]);
      i=1;
      j=5;
      k=6;
      p1+=-(F2[i]*F2[j]*(F1[k]-F2[k])+F2[i]*F2[k]*(F1[j]-F2[j])+F2[j]*F2[k]*(F1[i]-F2[i]))
      -2*a*(F2[i]*(F1[j]-F2[j])*(F1[k]-F2[k])+(F1[i]-F2[i])*F2[j]*(F1[k]-F2[k])+(F1[i]-F2[i])*(F1[j]-F2[j])*F2[k])
      -3*a*a*(F1[i]-F2[i])*(F1[j]-F2[j])*(F1[k]-F2[k]);
      i=2;
      j=3;
      k=7;
      p1+=-(F2[i]*F2[j]*(F1[k]-F2[k])+F2[i]*F2[k]*(F1[j]-F2[j])+F2[j]*F2[k]*(F1[i]-F2[i]))
      -2*a*(F2[i]*(F1[j]-F2[j])*(F1[k]-F2[k])+(F1[i]-F2[i])*F2[j]*(F1[k]-F2[k])+(F1[i]-F2[i])*(F1[j]-F2[j])*F2[k])
      -3*a*a*(F1[i]-F2[i])*(F1[j]-F2[j])*(F1[k]-F2[k]);
  
      a=a-p0/p1;
  }
  
    for(i=0;i<9;i++)
    {
        E[i]=a*F1[i]+(1-a)*F2[i];
      //(F2[i]+a*(F1[i]-F2[i])
    }
}
void calc_esse(double *q,double *t,double *pointsA,double *pointsB, double *X, double *res, double *dev, int num_points)
{
  double R1[9],R2[9],q1[4],q2[4],v[10],E[9];
  double lambda;
  int    cnt;

  calc_funda(E,res,dev, X,pointsA,pointsB, num_points);
//  printf("E=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);
//  printf("fdev: %f\n",sqrt(dev[0]));

//det(E)=-E[6]*E[4]*E[2]-E[7]*E[5]*E[0]-E[8]*E[3]*E[1]+E[0]*E[4]*E[8]+E[1]*E[5]*E[6]+E[2]*E[3]*E[7]
  decompose_E(R1,R2,t,E);
  rot_2_quater(q1,R1);
  rot_2_quater(q2,R2);
//  printf("t=[ %f %f %f]\n",t[0],t[1],t[2]);
//  printf("q1=[ %f %f %f %f]\n",q1[0],q1[1],q1[2],q1[3]);
//  printf("q2=[ %f %f %f %f]\n",q2[0],q2[1],q2[2],q2[3]);
  compose_E(E,q1,t);
//  printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);

//      printf("edev: %f \n",sqrt(dev[0]));
  cnt=0;
  v[7]=1.;
  v[8]=0.;
  v[9]=0.;
  v[0]=q2[0];
  v[1]=q2[1];
  v[2]=q2[2];
  v[3]=q2[3];
  v[4]=t[0];
  v[5]=t[1];
  v[6]=t[2];
  if(q1[0]>q2[0])
  {
      v[0]=q1[0];
      v[1]=q1[1];
      v[2]=q1[2];
      v[3]=q1[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];

      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
   //   printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
  /*    for(i=0;i<num_points;i++)
    printf("%f ",res[i]);
      printf("\n");*/
//      printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
    //  printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
  /*          for(i=0;i<num_points;i++)
    printf("%f ",res[i]);
      printf("\n");
            printf("2dev: %f \n",sqrt(dev[0]));*/
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
//      printf("cnt1: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
//     printf("cnt2: %d\n",cnt);
      }
  }
  if(cnt*2<num_points)
  {
      v[0]=q2[0];
      v[1]=q2[1];
      v[2]=q2[2];
      v[3]=q2[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
 //     printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("2dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
  //    printf("cnt3: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
//     printf("cnt4: %d\n",cnt);
      }
  }
//  printf("finalcnt: %d\n",cnt);
  t[0]=v[4];
  t[1]=v[5];
  t[2]=v[6];
  q[0]=v[0];
  q[1]=v[1];
  q[2]=v[2];
  q[3]=v[3];


}
void ransac_calc_esse7(double *q,double *t,double *pointsA,double *pointsB,  double *res, double *dev, double thres, int num_points)
{
  double R1[9],R2[9],q1[4],q2[4],v[10],E[9];
  double lambda;
  int    cnt;

  //if num_points>=7
  ransac_calc_funda7(E,dev,pointsA,pointsB, thres, num_points);
//  printf("E=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);
//  printf("fdev: %f\n",sqrt(dev[0]));

//det(E)=-E[6]*E[4]*E[2]-E[7]*E[5]*E[0]-E[8]*E[3]*E[1]+E[0]*E[4]*E[8]+E[1]*E[5]*E[6]+E[2]*E[3]*E[7]
  decompose_E(R1,R2,t,E);
  rot_2_quater(q1,R1);
  rot_2_quater(q2,R2);
//  printf("t=[ %f %f %f]\n",t[0],t[1],t[2]);
//  printf("q1=[ %f %f %f %f]\n",q1[0],q1[1],q1[2],q1[3]);
//  printf("q2=[ %f %f %f %f]\n",q2[0],q2[1],q2[2],q2[3]);
  compose_E(E,q1,t);
//  printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);

//      printf("edev: %f \n",sqrt(dev[0]));
  cnt=0;
  v[7]=1.;
  v[8]=0.;
  v[9]=0.;
  v[0]=q2[0];
  v[1]=q2[1];
  v[2]=q2[2];
  v[3]=q2[3];
  v[4]=t[0];
  v[5]=t[1];
  v[6]=t[2];
  if(q1[0]>q2[0])
  {
      v[0]=q1[0];
      v[1]=q1[1];
      v[2]=q1[2];
      v[3]=q1[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];

      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
   //   printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
  /*    for(i=0;i<num_points;i++)
    printf("%f ",res[i]);
      printf("\n");*/
//      printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
    //  printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
  /*          for(i=0;i<num_points;i++)
    printf("%f ",res[i]);
      printf("\n");
            printf("2dev: %f \n",sqrt(dev[0]));*/
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2.*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2.*sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
//      printf("cnt1: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
//     printf("cnt2: %d\n",cnt);
      }
  }
  if(cnt*2<num_points)
  {
      v[0]=q2[0];
      v[1]=q2[1];
      v[2]=q2[2];
      v[3]=q2[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
 //     printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("2dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
  //    printf("cnt3: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
//     printf("cnt4: %d\n",cnt);
      }
  }
//  printf("finalcnt: %d\n",cnt);
  t[0]=v[4];
  t[1]=v[5];
  t[2]=v[6];
  q[0]=v[0];
  q[1]=v[1];
  q[2]=v[2];
  q[3]=v[3];


}

void ransac_calc_esse(double *q,double *t,double *pointsA,double *pointsB,  double *res, double *dev, double thres, int num_points)
{
  double R1[9],R2[9],q1[4],q2[4],v[10],E[9];
  double lambda;
  int    cnt;

  ransac_calc_funda(E,dev,pointsA,pointsB, thres, num_points);
//  printf("E=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);
//  printf("fdev: %f\n",sqrt(dev[0]));

//det(E)=-E[6]*E[4]*E[2]-E[7]*E[5]*E[0]-E[8]*E[3]*E[1]+E[0]*E[4]*E[8]+E[1]*E[5]*E[6]+E[2]*E[3]*E[7]
  decompose_E(R1,R2,t,E);
  rot_2_quater(q1,R1);
  rot_2_quater(q2,R2);
//  printf("t=[ %f %f %f]\n",t[0],t[1],t[2]);
//  printf("q1=[ %f %f %f %f]\n",q1[0],q1[1],q1[2],q1[3]);
//  printf("q2=[ %f %f %f %f]\n",q2[0],q2[1],q2[2],q2[3]);
  compose_E(E,q1,t);
//  printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);

//      printf("edev: %f \n",sqrt(dev[0]));
  cnt=0;
  v[7]=1.;
  v[8]=0.;
  v[9]=0.;
  v[0]=q2[0];
  v[1]=q2[1];
  v[2]=q2[2];
  v[3]=q2[3];
  v[4]=t[0];
  v[5]=t[1];
  v[6]=t[2];
  if(q1[0]>q2[0])
  {
      v[0]=q1[0];
      v[1]=q1[1];
      v[2]=q1[2];
      v[3]=q1[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];

      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
   //   printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
  /*    for(i=0;i<num_points;i++)
	printf("%f ",res[i]);
      printf("\n");*/
//      printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
    //  printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
  /*          for(i=0;i<num_points;i++)
	printf("%f ",res[i]);
      printf("\n");
            printf("2dev: %f \n",sqrt(dev[0]));*/
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2.*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2.*sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
//      printf("cnt1: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
//	   printf("cnt2: %d\n",cnt);
      }
  }
  if(cnt*2<num_points)
  {
      v[0]=q2[0];
      v[1]=q2[1];
      v[2]=q2[2];
      v[3]=q2[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
 //     printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("2dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
//            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
  //    printf("cnt3: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
//	   printf("cnt4: %d\n",cnt);
      }
  }
//  printf("finalcnt: %d\n",cnt);

  t[0]=v[4];
  t[1]=v[5];
  t[2]=v[6];
  q[0]=v[0];
  q[1]=v[1];
  q[2]=v[2];
  q[3]=v[3];


}
void calc_esse1(double *q,double *t,double *pointsA,double *pointsB, double *X, double *res, double *dev, int num_points)
{
  double R1[9],R2[9],q1[4],q2[4],v[7],E[9];
  double lambda;
  int    cnt;

  calc_funda(E,res,dev, X,pointsA,pointsB, num_points);
  printf("E=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);
  printf("fdev: %f\n",sqrt(dev[0]));
  
//det(E)=-E[6]*E[4]*E[2]-E[7]*E[5]*E[0]-E[8]*E[3]*E[1]+E[0]*E[4]*E[8]+E[1]*E[5]*E[6]+E[2]*E[3]*E[7]
  decompose_E(R1,R2,t,E);
  rot_2_quater(q1,R1);
  rot_2_quater(q2,R2);
  printf("t=[ %f %f %f]\n",t[0],t[1],t[2]);
  printf("q1=[ %f %f %f %f]\n",q1[0],q1[1],q1[2],q1[3]);
  compose_E(E,q1,t);
  printf("E1=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
  calcdev(dev,res,E,pointsB,pointsA,num_points);

  printf("edev: %f \n",sqrt(dev[0]));
  cnt=0;
  if(q1[0]>q2[0])
  {
      v[0]=q1[0];
      v[1]=q1[1];
      v[2]=q1[2];
      v[3]=q1[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];

      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
      printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
            printf("2dev: %f \n",sqrt(dev[0]));
      esse_calct(v,pointsB,pointsA,&lambda,1.5*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,1.5*sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
      printf("cnt1: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
	   printf("cnt2: %d\n",cnt);
      }
  }
  if(cnt*2<num_points)
  {
      v[0]=q2[0];
      v[1]=q2[1];
      v[2]=q2[2];
      v[3]=q2[3];
      v[4]=t[0];
      v[5]=t[1];
      v[6]=t[2];
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
      printf("1dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,2*sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
            printf("2dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      compose_E(E,v,v+4);
      calcdev(dev,res,E,pointsB,pointsA,num_points);
            printf("3dev: %f \n",sqrt(dev[0]));
      lambda=0.0001;
      esse_calct(v,pointsB,pointsA,&lambda,sqrt(dev[0]),num_points);
      test_chirality(v,pointsA,pointsB,num_points, &cnt);
      printf("cnt3: %d\n",cnt);
      if(cnt*2<num_points)
      {
           v[4]=-v[4];
           v[5]=-v[5];
           v[6]=-v[6];
           test_chirality(v,pointsA,pointsB,num_points, &cnt);
           printf("cnt4: %d\n",cnt);
      }
  }
  printf("finalcnt: %d\n",cnt);
  t[0]=v[4];
  t[1]=v[5];
  t[2]=v[6];
  q[0]=v[0];
  q[1]=v[1];
  q[2]=v[2];
  q[3]=v[3];

}

void decompose_E(double *R1,double *R2,double *t,double *E)
{
    double S[3],U[9],V[9],detR;
    int l,k,Mind[3];
//    printf("E=[ %f %f %f %f %f %f %f %f %f]\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
    svduv(S,E,U,3,V,3);

    if(S[0]>=S[1])
    {
        if(S[0]>=S[2])
        {
            Mind[0]=0;
            if(S[1]>=S[2])
            {
                Mind[1]=1;
                Mind[2]=2;
            }
            else
            {
                Mind[1]=2;
                Mind[2]=1;
            }
        }
        else 
        {
            Mind[0]=2;
            Mind[1]=0;
            Mind[2]=1;
        }    
    }
    else
    {
        if(S[1]>=S[2])
        {
            Mind[0]=1;
            if(S[0]>=S[2])
            {
                Mind[1]=0;
                Mind[2]=2;
            }
            else
            {
                Mind[1]=2;
                Mind[2]=0;
            }
        }
        else 
        {
            Mind[0]=2;
            Mind[1]=1;
            Mind[2]=0;
        }    
    }           		
    /*R34=u*v';*/		
    for(k=0;k<3;k++)
        for(l=0;l<3;l++)
            R1[k+l*3]=V[k*3+Mind[0]]*U[Mind[1]+l*3]-V[3*k+Mind[1]]*U[Mind[0]+l*3]+V[3*k+Mind[2]]*U[Mind[2]+l*3];
    for(k=0;k<3;k++)
        for(l=0;l<3;l++)
            R2[k+l*3]=-V[k*3+Mind[0]]*U[Mind[1]+l*3]+V[3*k+Mind[1]]*U[Mind[0]+l*3]+V[3*k+Mind[2]]*U[Mind[2]+l*3];
 
  
    t[0]=V[0+Mind[2]];
    t[1]=V[3+Mind[2]];
    t[2]=V[6+Mind[2]];
    
    detR=-R1[6]*R1[4]*R1[2]-R1[7]*R1[5]*R1[0]-R1[8]*R1[3]*R1[1]+R1[0]*R1[4]*R1[8]+R1[1]*R1[5]*R1[6]+R1[2]*R1[3]*R1[7];	    
    if(detR<0)
    {
        R1[0]=-R1[0];
        R1[1]=-R1[1];
        R1[2]=-R1[2];
        R1[3]=-R1[3];
        R1[4]=-R1[4];
        R1[5]=-R1[5];
        R1[6]=-R1[6];
        R1[7]=-R1[7];
        R1[8]=-R1[8];
    }
    detR=-R2[6]*R2[4]*R2[2]-R2[7]*R2[5]*R2[0]-R2[8]*R2[3]*R2[1]+R2[0]*R2[4]*R2[8]+R2[1]*R2[5]*R2[6]+R2[2]*R2[3]*R2[7];	    
    if(detR<0)
    {
        R2[0]=-R2[0];
        R2[1]=-R2[1];
        R2[2]=-R2[2];
        R2[3]=-R2[3];
        R2[4]=-R2[4];
        R2[5]=-R2[5];
        R2[6]=-R2[6];
        R2[7]=-R2[7];
        R2[8]=-R2[8];
    }
}
void sample8(int *indo,int num_points)
{
    int cnt,incr,flag,i;
    
    cnt=0;
    
    while(cnt<8)
    {
        incr=(int)(((float)rand()/(float)RAND_MAX*(float)(num_points)));
        if(incr>num_points-1)
            incr=num_points-1;
        flag=1;
        for(i=0;i<cnt;i++)
        {
            if(incr==indo[i])
                flag=0;
        }
        if(flag)
        {
            indo[cnt]=incr;
            cnt++;
        }
    }
}
void sample7(int *indo,int num_points)
{
    int cnt,incr,flag,i;
    
    cnt=0;
    
    while(cnt<7)
    {
        incr=(int)(((float)rand()/(float)RAND_MAX*(float)(num_points)));
        if(incr>num_points-1)
            incr=num_points-1;
        flag=1;
        for(i=0;i<cnt;i++)
        {
            if(incr==indo[i])
                flag=0;
        }
        if(flag)
        {
            indo[cnt]=incr;
            cnt++;
        }
    }
}
void calc_funda(double *F, double *res, double *devmin, double *X,double *pointsA,double  *pointsB, int num_points)
{
    int i,j,k,incr;
    double dev,funda[9],S[32],Xi[128],V[128],U[128],w[128],R[9],R1[9],t[3];


    //X=[feasA(1,:).*feasB(1,:);feasA(1,:).*feasB(2,:);feasA(1,:);feasA(2,:).*feasB(1,:);feasA(2,:).*feasB(2,:);feasA(2,:);feasB(1,:);feasB(2,:);ones(size(feasB(2,:)))];

    //X=[fA(1,:).*fB(1,:);fA(1,:).*fB(2,:);fA(1,:);fA(2,:).*fB(1,:);fA(2,:).*fB(2,:);fA(2,:);fB(1,:);fB(2,:);ones(size(fB(2,:)))]

    for(i=0;i<num_points;i++){
        X[0+i*9]=pointsA[2*i]*pointsB[2*i];
        X[1+i*9]=pointsA[2*i]*pointsB[2*i+1];
        X[2+i*9]=pointsA[2*i];
        X[3+i*9]=pointsA[2*i+1]*pointsB[2*i];
        X[4+i*9]=pointsA[2*i+1]*pointsB[2*i+1];
        X[5+i*9]=pointsA[2*i+1];
        X[6+i*9]=pointsB[2*i];
        X[7+i*9]=pointsB[2*i+1];
        X[8+i*9]=1.;
    }
  
    *devmin=10000.0;

    for(i=0;i<100;i++)
    {
        j=-1;
        for(k=0;k<8;k++)
        {
            incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/8.);
            if(incr<1)
                incr=1;

            j=j+incr;
            if(j>=num_points)
                j=num_points-1;
            Xi[k]=X[0+j*9];
            Xi[k+1*8]=X[1+j*9];
            Xi[k+2*8]=X[2+j*9];
            Xi[k+3*8]=X[3+j*9];
            Xi[k+4*8]=X[4+j*9];
            Xi[k+5*8]=X[5+j*9];
            Xi[k+6*8]=X[6+j*9];
            Xi[k+7*8]=X[7+j*9];
            Xi[k+8*8]=X[8+j*9];
        }
        calc_qr(V,U,Xi,S,w,9,8);
        j=8;
        funda[0]=U[j];
        funda[1]=U[9*1+j];
        funda[2]=U[9*2+j];
        funda[3]=U[9*3+j];
        funda[4]=U[9*4+j];
        funda[5]=U[9*5+j];
        funda[6]=U[9*6+j];
        funda[7]=U[9*7+j];
        funda[8]=U[9*8+j];
        decompose_E(R,R1,t,funda); /*fixme: heavy */
        funda[0]=t[1]*R[2]-t[2]*R[1];
        funda[1]=t[2]*R[0]-t[0]*R[2];
        funda[2]=t[0]*R[1]-t[1]*R[0];
        funda[3]=t[1]*R[5]-t[2]*R[4];
        funda[4]=t[2]*R[3]-t[0]*R[5];
        funda[5]=t[0]*R[4]-t[1]*R[3];
        funda[6]=t[1]*R[8]-t[2]*R[7];
        funda[7]=t[2]*R[6]-t[0]*R[8];
        funda[8]=t[0]*R[7]-t[1]*R[6];
        calcdev(&dev,res,funda,pointsB,pointsA,num_points);

        if(dev<*devmin)
        {
           *devmin=dev;
            F[0]=funda[0];
            F[1]=funda[1];
            F[2]=funda[2];
            F[3]=funda[3];
            F[4]=funda[4];
            F[5]=funda[5];
            F[6]=funda[6];
            F[7]=funda[7];
            F[8]=funda[8];
        }    
    }
}
void ransac_calc_funda(double *F, double *devmin,double *pointsA,double  *pointsB, double thres, int num_points)
{
    int i,j,k,indo[8];
    double dev,funda[9],S[32],Xi[128],V[128],U[128],w[128],R[9],R1[9],t[3];


    //X=[feasA(1,:).*feasB(1,:);feasA(1,:).*feasB(2,:);feasA(1,:);feasA(2,:).*feasB(1,:);feasA(2,:).*feasB(2,:);feasA(2,:);feasB(1,:);feasB(2,:);ones(size(feasB(2,:)))];

    //X=[fA(1,:).*fB(1,:);fA(1,:).*fB(2,:);fA(1,:);fA(2,:).*fB(1,:);fA(2,:).*fB(2,:);fA(2,:);fB(1,:);fB(2,:);ones(size(fB(2,:)))]

    *devmin=thres*num_points+1;

    for(i=0;i<500;i++)
    {
        sample8(indo,num_points);
        for(k=0;k<8;k++)
        {
            Xi[k]=pointsA[2*indo[k]]*pointsB[2*indo[k]];
            Xi[k+8]=pointsA[2*indo[k]]*pointsB[2*indo[k]+1];
            Xi[k+2*8]=pointsA[2*indo[k]];
            Xi[k+3*8]=pointsA[2*indo[k]+1]*pointsB[2*indo[k]];
            Xi[k+4*8]=pointsA[2*indo[k]+1]*pointsB[2*indo[k]+1];
            Xi[k+5*8]=pointsA[2*indo[k]+1];
            Xi[k+6*8]=pointsB[2*indo[k]];
            Xi[k+7*8]=pointsB[2*indo[k]+1];
            Xi[k+8*8]=1.;
        }
        calc_qr(V,U,Xi,S,w,9,8);
        j=8;
        funda[0]=U[j];
        funda[1]=U[9*1+j];
        funda[2]=U[9*2+j];
        funda[3]=U[9*3+j];
        funda[4]=U[9*4+j];
        funda[5]=U[9*5+j];
        funda[6]=U[9*6+j];
        funda[7]=U[9*7+j];
        funda[8]=U[9*8+j];
        decompose_E(R,R1,t,funda); /*fixme: heavy */
        funda[0]=t[1]*R[2]-t[2]*R[1];
        funda[1]=t[2]*R[0]-t[0]*R[2];
        funda[2]=t[0]*R[1]-t[1]*R[0];
        funda[3]=t[1]*R[5]-t[2]*R[4];
        funda[4]=t[2]*R[3]-t[0]*R[5];
        funda[5]=t[0]*R[4]-t[1]*R[3];
        funda[6]=t[1]*R[8]-t[2]*R[7];
        funda[7]=t[2]*R[6]-t[0]*R[8];
        funda[8]=t[0]*R[7]-t[1]*R[6];
        calcdevt(&dev,funda,pointsB,pointsA,thres,num_points);

        if(dev<*devmin)
        {
           *devmin=dev;
            F[0]=funda[0];
            F[1]=funda[1];
            F[2]=funda[2];
            F[3]=funda[3];
            F[4]=funda[4];
            F[5]=funda[5];
            F[6]=funda[6];
            F[7]=funda[7];
            F[8]=funda[8];
        }    
    }
}
void ransac_calc_funda7(double *F, double *devmin,double *pointsA,double  *pointsB, double thres, int num_points)
{
    int i,j,k,indo[8];
    double dev,funda[9],F1[9],F2[9],S[32],Xi[128],V[128],U[128],w[128],R[9],R1[9],t[3];


    //X=[feasA(1,:).*feasB(1,:);feasA(1,:).*feasB(2,:);feasA(1,:);feasA(2,:).*feasB(1,:);feasA(2,:).*feasB(2,:);feasA(2,:);feasB(1,:);feasB(2,:);ones(size(feasB(2,:)))];

    //X=[fA(1,:).*fB(1,:);fA(1,:).*fB(2,:);fA(1,:);fA(2,:).*fB(1,:);fA(2,:).*fB(2,:);fA(2,:);fB(1,:);fB(2,:);ones(size(fB(2,:)))]

    *devmin=thres*num_points+1;

    for(i=0;i<500;i++)
    {
        sample7(indo,num_points);
        for(k=0;k<7;k++)
        {
            Xi[k*9]=pointsA[2*indo[k]]*pointsB[2*indo[k]];
            Xi[k*9+1]=pointsA[2*indo[k]]*pointsB[2*indo[k]+1];
            Xi[k*9+2]=pointsA[2*indo[k]];
            Xi[k*9+3]=pointsA[2*indo[k]+1]*pointsB[2*indo[k]];
            Xi[k*9+4]=pointsA[2*indo[k]+1]*pointsB[2*indo[k]+1];
            Xi[k*9+5]=pointsA[2*indo[k]+1];
            Xi[k*9+6]=pointsB[2*indo[k]];
            Xi[k*9+7]=pointsB[2*indo[k]+1];
            Xi[k*9+8]=1.;
        }

        calc_qr(V,U,Xi,S,w,9,7);

        j=7;
        F1[0]=U[9*j];
        F1[1]=U[1+9*j];
        F1[2]=U[2+9*j];
        F1[3]=U[3+9*j];
        F1[4]=U[4+9*j];
        F1[5]=U[5+9*j];
        F1[6]=U[6+9*j];
        F1[7]=U[7+9*j];
        F1[8]=U[8+9*j];
 //       printf("F1=[ %lf %lf %lf %lf %lf %lf %lf %lf %lf]\n",F1[0],F1[1],F1[2],F1[3],F1[4],F1[5],F1[6],F1[7],F1[8]);
        j=8;
        F2[0]=U[9*j];
        F2[1]=U[1+9*j];
        F2[2]=U[2+9*j];
        F2[3]=U[3+9*j];
        F2[4]=U[4+9*j];
        F2[5]=U[5+9*j];
        F2[6]=U[6+9*j];
        F2[7]=U[7+9*j];
        F2[8]=U[8+9*j];
//        printf("F2=[ %lf %lf %lf %lf %lf %lf %lf %lf %lf]\n",F2[0],F2[1],F2[2],F2[3],F2[4],F2[5],F2[6],F2[7],F2[8]);

        find_zero_det_F(funda,F1,F2);
//         printf("F3=[ %lf %lf %lf %lf %lf %lf %lf %lf %lf]\n",funda[0],funda[1],funda[2],funda[3],funda[4],funda[5],funda[6],funda[7],funda[8]);
        decompose_E(R,R1,t,funda); /*fixme: heavy */
//        printf("t=[%lf %lf %lf\n]",t[0],t[1],t[2]);
//        printf("R=[ %lf %lf %lf %lf %lf %lf %lf %lf %lf]\n",R[0],R[1],R[2],R[3],R[4],R[5],R[6],R[7],R[8]);
        funda[0]=t[1]*R[2]-t[2]*R[1];
        funda[1]=t[2]*R[0]-t[0]*R[2];
        funda[2]=t[0]*R[1]-t[1]*R[0];
        funda[3]=t[1]*R[5]-t[2]*R[4];
        funda[4]=t[2]*R[3]-t[0]*R[5];
        funda[5]=t[0]*R[4]-t[1]*R[3];
        funda[6]=t[1]*R[8]-t[2]*R[7];
        funda[7]=t[2]*R[6]-t[0]*R[8];
        funda[8]=t[0]*R[7]-t[1]*R[6];
        calcdevt(&dev,funda,pointsB,pointsA,thres,num_points);

        if(dev<*devmin)
        {
           *devmin=dev;
            F[0]=funda[0];
            F[1]=funda[1];
            F[2]=funda[2];
            F[3]=funda[3];
            F[4]=funda[4];
            F[5]=funda[5];
            F[6]=funda[6];
            F[7]=funda[7];
            F[8]=funda[8];
        }    
    }
}

void calc_funda2(double *F, double *res, double *devmin, double *X,double *pointsA,double  *pointsB, int num_points)
{
    int i,j,k,incr;
    double dev,funda[9],mins,S[32],Xi[128],V[128],U[128],R[9],R1[9],t[3];


    //X=[feasA(1,:).*feasB(1,:);feasA(1,:).*feasB(2,:);feasA(1,:);feasA(2,:).*feasB(1,:);feasA(2,:).*feasB(2,:);feasA(2,:);feasB(1,:);feasB(2,:);ones(size(feasB(2,:)))];

    //X=[fA(1,:).*fB(1,:);fA(1,:).*fB(2,:);fA(1,:);fA(2,:).*fB(1,:);fA(2,:).*fB(2,:);fA(2,:);fB(1,:);fB(2,:);ones(size(fB(2,:)))]

    for(i=0;i<num_points;i++){
        X[0+i*9]=pointsA[2*i]*pointsB[2*i];
        X[1+i*9]=pointsA[2*i]*pointsB[2*i+1];
        X[2+i*9]=pointsA[2*i];
        X[3+i*9]=pointsA[2*i+1]*pointsB[2*i];
        X[4+i*9]=pointsA[2*i+1]*pointsB[2*i+1];
        X[5+i*9]=pointsA[2*i+1];
        X[6+i*9]=pointsB[2*i];
        X[7+i*9]=pointsB[2*i+1];
        X[8+i*9]=1.;
    }
  
    *devmin=10000.0;

    for(i=0;i<100;i++)
    {
        j=-1;
        for(k=0;k<8;k++)
        {
            incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/8.);
            if(incr<1)
                incr=1;

            j=j+incr;
            if(j>=num_points)
                j=num_points-1;
            Xi[k]=X[0+j*9];
            Xi[k+1*8]=X[1+j*9];
            Xi[k+2*8]=X[2+j*9];
            Xi[k+3*8]=X[3+j*9];
            Xi[k+4*8]=X[4+j*9];
            Xi[k+5*8]=X[5+j*9];
            Xi[k+6*8]=X[6+j*9];
            Xi[k+7*8]=X[7+j*9];
            Xi[k+8*8]=X[8+j*9];
        }

       svduv(S,Xi,U,9,V,8);
  
        j=0;
        mins=S[j];

        for(k=1;k<8;k++)
        {
            if(mins>S[k])
            {
                j=k;
                mins=S[k];
            }
        }

        funda[0]=U[j];
        funda[1]=U[9*1+j];
        funda[2]=U[9*2+j];
        funda[3]=U[9*3+j];
        funda[4]=U[9*4+j];
        funda[5]=U[9*5+j];
        funda[6]=U[9*6+j];
        funda[7]=U[9*7+j];
        funda[8]=U[9*8+j];
        decompose_E(R,R1,t,funda); /*fixme: heavy */
        funda[0]=t[1]*R[2]-t[2]*R[1];
        funda[1]=t[2]*R[0]-t[0]*R[2];
        funda[2]=t[0]*R[1]-t[1]*R[0];
        funda[3]=t[1]*R[5]-t[2]*R[4];
        funda[4]=t[2]*R[3]-t[0]*R[5];
        funda[5]=t[0]*R[4]-t[1]*R[3];
        funda[6]=t[1]*R[8]-t[2]*R[7];
        funda[7]=t[2]*R[6]-t[0]*R[8];
        funda[8]=t[0]*R[7]-t[1]*R[6];
        calcdev(&dev,res,funda,pointsB,pointsA,num_points);

        if(dev<*devmin)
        {
           *devmin=dev;
            F[0]=funda[0];
            F[1]=funda[1];
            F[2]=funda[2];
            F[3]=funda[3];
            F[4]=funda[4];
            F[5]=funda[5];
            F[6]=funda[6];
            F[7]=funda[7];
            F[8]=funda[8];
        }    
    }
}
void calc_funda1(double *F, double *res, double *devmin, double *X,double *pointsA,double  *pointsB, int num_points)
{
    int i,j,k,incr;
    double dev,funda[9],mins,S[32],Xi[128],V[128],U[128];


    //X=[feasA(1,:).*feasB(1,:);feasA(1,:).*feasB(2,:);feasA(1,:);feasA(2,:).*feasB(1,:);feasA(2,:).*feasB(2,:);feasA(2,:);feasB(1,:);feasB(2,:);ones(size(feasB(2,:)))];

    //X=[fA(1,:).*fB(1,:);fA(1,:).*fB(2,:);fA(1,:);fA(2,:).*fB(1,:);fA(2,:).*fB(2,:);fA(2,:);fB(1,:);fB(2,:);ones(size(fB(2,:)))]

    for(i=0;i<num_points;i++){
        X[0+i*9]=pointsA[2*i]*pointsB[2*i];
        X[1+i*9]=pointsA[2*i]*pointsB[2*i+1];
        X[2+i*9]=pointsA[2*i];
        X[3+i*9]=pointsA[2*i+1]*pointsB[2*i];
        X[4+i*9]=pointsA[2*i+1]*pointsB[2*i+1];
        X[5+i*9]=pointsA[2*i+1];
        X[6+i*9]=pointsB[2*i];
        X[7+i*9]=pointsB[2*i+1];
        X[8+i*9]=1.;
    }
  
    *devmin=10000.0;

    for(i=0;i<100;i++)
    {
        j=-1;
        for(k=0;k<8;k++)
        {
            incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/8.);
            if(incr<1)
                incr=1;

            j=j+incr;
            if(j>=num_points)
                j=num_points-1;
            Xi[k]=X[0+j*9];
            Xi[k+1*8]=X[1+j*9];
            Xi[k+2*8]=X[2+j*9];
            Xi[k+3*8]=X[3+j*9];
            Xi[k+4*8]=X[4+j*9];
            Xi[k+5*8]=X[5+j*9];
            Xi[k+6*8]=X[6+j*9];
            Xi[k+7*8]=X[7+j*9];
            Xi[k+8*8]=X[8+j*9];
        }

       svduv(S,Xi,U,9,V,8);
  
        j=0;
        mins=S[j];

        for(k=1;k<9;k++)
        {
            if(mins>S[k])
            {
                j=k;
                mins=S[k];
            }
        }

        funda[0]=U[j];
        funda[1]=U[9*1+j];
        funda[2]=U[9*2+j];
        funda[3]=U[9*3+j];
        funda[4]=U[9*4+j];
        funda[5]=U[9*5+j];
        funda[6]=U[9*6+j];
        funda[7]=U[9*7+j];
        funda[8]=U[9*8+j];
      
        calcdev(&dev,res,funda,pointsB,pointsA,num_points);

        if(dev<*devmin)
        {
           *devmin=dev;
            F[0]=funda[0];
            F[1]=funda[1];
            F[2]=funda[2];
            F[3]=funda[3];
            F[4]=funda[4];
            F[5]=funda[5];
            F[6]=funda[6];
            F[7]=funda[7];
            F[8]=funda[8];
        }    
    }
}


void calcdev(double *dev,double *res, double *F, double *pointsA, double  *pointsB, int num_points)
{
    int i;
    double a1,b1,c1,w,d;


    for(i=0;i<num_points;i++){
        a1=(F[0]*pointsA[2*i]+F[1]*pointsA[2*i+1]+F[2]);
        b1=(F[3]*pointsA[2*i]+F[4]*pointsA[2*i+1]+F[5]);
        c1=(F[6]*pointsA[2*i]+F[7]*pointsA[2*i+1]+F[8]);

        w=sqrt(1/(a1*a1+b1*b1));//+sqrt(1/(a2*a2+b2*b2));
        d=a1*pointsB[2*i]+b1*pointsB[2*i+1]+c1;
        res[i]=w*w*d*d;
    }
    jsort(res,num_points);
    *dev=res[(int)(num_points/2)+1];

}
void calcdevt(double *dev, double *F, double *pointsA, double  *pointsB, double thres, int num_points)
{
    int i;
    double a1,b1,c1,w,d,er,ssd;

    ssd=0;
    for(i=0;i<num_points;i++){
        a1=(F[0]*pointsA[2*i]+F[1]*pointsA[2*i+1]+F[2]);
        b1=(F[3]*pointsA[2*i]+F[4]*pointsA[2*i+1]+F[5]);
        c1=(F[6]*pointsA[2*i]+F[7]*pointsA[2*i+1]+F[8]);

        w=sqrt(1/(a1*a1+b1*b1));//+sqrt(1/(a2*a2+b2*b2));
        d=a1*pointsB[2*i]+b1*pointsB[2*i+1]+c1;
        er=w*w*d*d;        
        if(er>thres)
            er=thres;
        ssd+=er;
    }
    *dev=ssd;

}
void calcdev1(double *dev,double *res, double *F, double *pointsA, double  *pointsB,double *p, int num_points)
{
    int i;
    double a1,b1,c1,w,d,pA[2],pB[2];


    for(i=0;i<num_points;i++){
        pA[0]=(pointsA[2*i]-p[1])/p[0];
        pA[1]=(pointsA[2*i+1]-p[2])/p[0];
        pB[0]=(pointsB[2*i]-p[1])/p[0];
        pB[1]=(pointsB[2*i+1]-p[2])/p[0];
        a1=(F[0]*pA[0]+F[1]*pA[1]+F[2]);
        b1=(F[3]*pA[0]+F[4]*pA[1]+F[5]);
        c1=(F[6]*pA[0]+F[7]*pA[1]+F[8]);

        w=sqrt(1/(a1*a1+b1*b1))*p[0];//+sqrt(1/(a2*a2+b2*b2));
        d=a1*pB[0]+b1*pB[1]+c1;
        res[i]=w*w*d*d;
    }
    jsort(res,num_points);
    *dev=res[(int)(num_points/2)+1];

}
void calcdevh(double *dev,double *res, double *H, double *pointsA,double  *pointsB, int num_points)
{
    int i;
    double a1,b1,c1,d1,d2;


    for(i=0;i<num_points;i++){
        a1=H[0]*pointsA[2*i]+H[3]*pointsA[2*i+1]+H[6];
        b1=H[1]*pointsA[2*i]+H[4]*pointsA[2*i+1]+H[7];
        c1=H[2]*pointsA[2*i]+H[5]*pointsA[2*i+1]+H[8];
      /*  a1=H[0]*pointsA[2*i]+H[1]*pointsA[2*i+1]+H[2];
        b1=H[3]*pointsA[2*i]+H[4]*pointsA[2*i+1]+H[5];
        c1=H[6]*pointsA[2*i]+H[7]*pointsA[2*i+1]+H[8];*/
        
        d1=pointsB[2*i]-a1/c1;
        d2=pointsB[2*i+1]-b1/c1;
        
        res[i]=(d1*d1+d2*d2);
    }
    jsort(res,num_points);
    *dev=res[(int)(num_points/2)+1];

}
void calcdevp(double *dev,double *res, double *H, double *pointsA,double  *pointsB, int num_points,double *p)
{
    int i;
    double a1,b1,c1,d1,d2,pA[2],pB[2];


    for(i=0;i<num_points;i++){
        pA[0]=(pointsA[2*i]-p[1])/p[0];
        pA[1]=(pointsA[2*i+1]-p[2])/p[0];
        a1=H[0]*pA[0]+H[3]*pA[1]+H[6];
        b1=H[1]*pA[0]+H[4]*pA[1]+H[7];
        c1=H[2]*pA[0]+H[5]*pA[1]+H[8];
      /*  a1=H[0]*pointsA[2*i]+H[1]*pointsA[2*i+1]+H[2];
        b1=H[3]*pointsA[2*i]+H[4]*pointsA[2*i+1]+H[5];
        c1=H[6]*pointsA[2*i]+H[7]*pointsA[2*i+1]+H[8];*/
        pB[0]=(pointsB[2*i]-p[1])/p[0];
        pB[1]=(pointsB[2*i+1]-p[2])/p[0];
        d1=pB[0]-a1/c1;
        d2=pB[1]-b1/c1;
        
        res[i]=(d1*d1+d2*d2);
    }
    jsort(res,num_points);
    *dev=res[(int)(num_points/2)+1];

}
void sample3(int *indo,int num_points)
{
    int cnt,incr,flag,i;
    
    cnt=0;
    
    while(cnt<3)
    {
        incr=(int)(((float)rand()/(float)RAND_MAX*(float)(num_points)));
        if(incr>num_points-1)
            incr=num_points-1;
        flag=1;
        for(i=0;i<cnt;i++)
        {
            if(incr==indo[i])
                flag=0;
        }
        if(flag)
        {
            indo[cnt]=incr;
            cnt++;
        }
    }
}

void calc_p3p(double *q,double *t,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points)
{
    double Rt[9],qt[4],nq1,nq2,nq3;
    int i,j,k,incr,cnt;
    double dev,p[9],q1[3],q2[3],q3[3];
    double tt[3],x[12],X[16];

    *devmin=100000000.0;
    for(i=0;i<600;i++){
        j=-1;

        incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/3.);
        if(incr==0)
            incr=1;
        
        j=j+incr;

        q1[0]=(pointsA[j*2]-cal[1])/cal[0];
        q1[1]=(pointsA[j*2+1]-cal[2])/cal[0];

        p[0]=pointsB[j*3];
        p[1]=pointsB[j*3+1];
        p[2]=pointsB[j*3+2];
        
        incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/3.);
        if(incr==0)
            incr=1;

        j=j+incr;
        q2[0]=(pointsA[j*2]-cal[1])/cal[0];
        q2[1]=(pointsA[j*2+1]-cal[2])/cal[0];

        p[3]=pointsB[j*3];
        p[4]=pointsB[j*3+1];
        p[5]=pointsB[j*3+2];

        incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/3.);
        if(incr==0)
            incr=1;
		
        j=j+incr;
        q3[0]=(pointsA[j*2]-cal[1])/cal[0];
        q3[1]=(pointsA[j*2+1]-cal[2])/cal[0];

        p[6]=pointsB[j*3];
        p[7]=pointsB[j*3+1];
        p[8]=pointsB[j*3+2];
    
        nq1=sqrt(q1[0]*q1[0]+q1[1]*q1[1]+1);
        nq2=sqrt(q2[0]*q2[0]+q2[1]*q2[1]+1);
        nq3=sqrt(q3[0]*q3[0]+q3[1]*q3[1]+1);

        q1[0]=q1[0]/nq1;
        q1[1]=q1[1]/nq1;
        q1[2]=1./nq1;
        q2[0]=q2[0]/nq2;
        q2[1]=q2[1]/nq2;
        q2[2]=1./nq2;
        q3[0]=q3[0]/nq3;
        q3[1]=q3[1]/nq3;
        q3[2]=1./nq3;

        p3p1(x,q1,q2,q3,p,p+3,p+6,&cnt);

        for(k=0;k<cnt;k++)
        {
            p3p2(x+k*3,q1,q2,q3,X,X+3,X+6);
            calc_p3pRt(qt,Rt,tt,X,p);
            p3p2dev(&dev,res,Rt,tt,cal,pointsA,pointsB,num_points);
            if(dev<(*devmin))
            {
                q[0]=qt[0];
                q[1]=qt[1];
                q[2]=qt[2];
                q[3]=qt[3];
                t[0]=tt[0];
                t[1]=tt[1];
                t[2]=tt[2];
                (*devmin)=dev;
            }
        }
    }
}

void ransac_calc_p3p1(double *q,double *t,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points, double thres)
{
    double Rt[9],qt[4],nq1,nq2,nq3;
    int i,k,cnt,indo[3];
    double dev,p[9],q1[3],q2[3],q3[3];
    double tt[3],x[12],X[16];

    *devmin=100000000.0;
    for(i=0;i<600;i++){

        sample3(indo,num_points);
   

        q1[0]=(pointsA[indo[0]*2]-cal[1])/cal[0];
        q1[1]=(pointsA[indo[0]*2+1]-cal[2])/cal[0];

        p[0]=pointsB[indo[0]*3];
        p[1]=pointsB[indo[0]*3+1];
        p[2]=pointsB[indo[0]*3+2];
        

        q2[0]=(pointsA[indo[1]*2]-cal[1])/cal[0];
        q2[1]=(pointsA[indo[1]*2+1]-cal[2])/cal[0];

        p[3]=pointsB[indo[1]*3];
        p[4]=pointsB[indo[1]*3+1];
        p[5]=pointsB[indo[1]*3+2];


        q3[0]=(pointsA[indo[2]*2]-cal[1])/cal[0];
        q3[1]=(pointsA[indo[2]*2+1]-cal[2])/cal[0];

        p[6]=pointsB[indo[2]*3];
        p[7]=pointsB[indo[2]*3+1];
        p[8]=pointsB[indo[2]*3+2];
    
        nq1=sqrt(q1[0]*q1[0]+q1[1]*q1[1]+1);
        nq2=sqrt(q2[0]*q2[0]+q2[1]*q2[1]+1);
        nq3=sqrt(q3[0]*q3[0]+q3[1]*q3[1]+1);

        q1[0]=q1[0]/nq1;
        q1[1]=q1[1]/nq1;
        q1[2]=1./nq1;
        q2[0]=q2[0]/nq2;
        q2[1]=q2[1]/nq2;
        q2[2]=1./nq2;
        q3[0]=q3[0]/nq3;
        q3[1]=q3[1]/nq3;
        q3[2]=1./nq3;

        p3p1(x,q1,q2,q3,p,p+3,p+6,&cnt);

        for(k=0;k<cnt;k++)
        {
            p3p2(x+k*3,q1,q2,q3,X,X+3,X+6);
            calc_p3pRt1(qt,Rt,tt,X,p);
            p3p2devt1(&dev,res,Rt,tt,cal,pointsA,pointsB,num_points,thres);
            if(dev<(*devmin))
            {
                q[0]=qt[0];
                q[1]=qt[1];
                q[2]=qt[2];
                q[3]=qt[3];
                t[0]=tt[0];
                t[1]=tt[1];
                t[2]=tt[2];
                (*devmin)=dev;
            }
        }
    }
}

void ransac_calc_p3p(double *q,double *t,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points, double thres)
{
    double Rt[9],qt[4],nq1,nq2,nq3;
    int i,k,cnt,indo[3];
    double dev,p[9],q1[3],q2[3],q3[3];
    double tt[3],x[12],X[16];

    *devmin=100000000.0;
    for(i=0;i<2000;i++){

        sample3(indo,num_points);
   

        q1[0]=(pointsA[indo[0]*2]-cal[1])/cal[0];
        q1[1]=(pointsA[indo[0]*2+1]-cal[2])/cal[0];

        p[0]=pointsB[indo[0]*3];
        p[1]=pointsB[indo[0]*3+1];
        p[2]=pointsB[indo[0]*3+2];
        

        q2[0]=(pointsA[indo[1]*2]-cal[1])/cal[0];
        q2[1]=(pointsA[indo[1]*2+1]-cal[2])/cal[0];

        p[3]=pointsB[indo[1]*3];
        p[4]=pointsB[indo[1]*3+1];
        p[5]=pointsB[indo[1]*3+2];


        q3[0]=(pointsA[indo[2]*2]-cal[1])/cal[0];
        q3[1]=(pointsA[indo[2]*2+1]-cal[2])/cal[0];

        p[6]=pointsB[indo[2]*3];
        p[7]=pointsB[indo[2]*3+1];
        p[8]=pointsB[indo[2]*3+2];
    
        nq1=sqrt(q1[0]*q1[0]+q1[1]*q1[1]+1);
        nq2=sqrt(q2[0]*q2[0]+q2[1]*q2[1]+1);
        nq3=sqrt(q3[0]*q3[0]+q3[1]*q3[1]+1);

        q1[0]=q1[0]/nq1;
        q1[1]=q1[1]/nq1;
        q1[2]=1./nq1;
        q2[0]=q2[0]/nq2;
        q2[1]=q2[1]/nq2;
        q2[2]=1./nq2;
        q3[0]=q3[0]/nq3;
        q3[1]=q3[1]/nq3;
        q3[2]=1./nq3;

        p3p1(x,q1,q2,q3,p,p+3,p+6,&cnt);

        for(k=0;k<cnt;k++)
        {
            p3p2(x+k*3,q1,q2,q3,X,X+3,X+6);
            calc_p3pRt(qt,Rt,tt,X,p);
            p3p2devt(&dev,res,Rt,tt,cal,pointsA,pointsB,num_points,thres);
            if(dev<(*devmin))
            {
                q[0]=qt[0];
                q[1]=qt[1];
                q[2]=qt[2];
                q[3]=qt[3];
                t[0]=tt[0];
                t[1]=tt[1];
                t[2]=tt[2];
                (*devmin)=dev;
            }
        }
    }
}

void ransac_calc_p3p_v(double *q,double *t,double *oq,double *ot,double dq, double dt,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points, double thres)
{
    double Rt[9],qt[4],nq1,nq2,nq3;
    int i,k,cnt,indo[3];
    double dev,p[9],q1[3],q2[3],q3[3];
    double tt[3],x[12],X[16];

    *devmin=100000000.0;
    for(i=0;i<2000;i++){

        sample3(indo,num_points);
   

        q1[0]=(pointsA[indo[0]*2]-cal[1])/cal[0];
        q1[1]=(pointsA[indo[0]*2+1]-cal[2])/cal[0];

        p[0]=pointsB[indo[0]*3];
        p[1]=pointsB[indo[0]*3+1];
        p[2]=pointsB[indo[0]*3+2];
        

        q2[0]=(pointsA[indo[1]*2]-cal[1])/cal[0];
        q2[1]=(pointsA[indo[1]*2+1]-cal[2])/cal[0];

        p[3]=pointsB[indo[1]*3];
        p[4]=pointsB[indo[1]*3+1];
        p[5]=pointsB[indo[1]*3+2];


        q3[0]=(pointsA[indo[2]*2]-cal[1])/cal[0];
        q3[1]=(pointsA[indo[2]*2+1]-cal[2])/cal[0];

        p[6]=pointsB[indo[2]*3];
        p[7]=pointsB[indo[2]*3+1];
        p[8]=pointsB[indo[2]*3+2];
    
        nq1=sqrt(q1[0]*q1[0]+q1[1]*q1[1]+1);
        nq2=sqrt(q2[0]*q2[0]+q2[1]*q2[1]+1);
        nq3=sqrt(q3[0]*q3[0]+q3[1]*q3[1]+1);

        q1[0]=q1[0]/nq1;
        q1[1]=q1[1]/nq1;
        q1[2]=1./nq1;
        q2[0]=q2[0]/nq2;
        q2[1]=q2[1]/nq2;
        q2[2]=1./nq2;
        q3[0]=q3[0]/nq3;
        q3[1]=q3[1]/nq3;
        q3[2]=1./nq3;

        p3p1(x,q1,q2,q3,p,p+3,p+6,&cnt);

        for(k=0;k<cnt;k++)
        {
            p3p2(x+k*3,q1,q2,q3,X,X+3,X+6);
            calc_p3pRt(qt,Rt,tt,X,p);
            if((qt[0]*oq[0]+qt[1]*oq[1]+qt[2]*oq[2]+qt[3]*oq[3])>dq)
            {
                p3p2devt(&dev,res,Rt,tt,cal,pointsA,pointsB,num_points,thres);
                if(dev<(*devmin))
                {
                    q[0]=qt[0];
                    q[1]=qt[1];
                    q[2]=qt[2];
                    q[3]=qt[3];
                    t[0]=tt[0];
                    t[1]=tt[1];
                    t[2]=tt[2];
                    (*devmin)=dev;
                }
            }
        }
    }
}

void ransac_calc_p3p_vl(double *q,double *t,double *oq,double *ot,double dq, double dt,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points, double thres,double  scale)
{
    double Rt[9],qt[4],nq1,nq2,nq3;
    int i,k,cnt,indo[3];
    double dev,p[9],q1[3],q2[3],q3[3];
    double tt[3],x[12],X[16],length;

    *devmin=100000000.0;
    for(i=0;i<2000;i++){

        sample3(indo,num_points);
   

        q1[0]=(pointsA[indo[0]*2]-cal[1])/cal[0];
        q1[1]=(pointsA[indo[0]*2+1]-cal[2])/cal[0];

        p[0]=pointsB[indo[0]*3];
        p[1]=pointsB[indo[0]*3+1];
        p[2]=pointsB[indo[0]*3+2];
        

        q2[0]=(pointsA[indo[1]*2]-cal[1])/cal[0];
        q2[1]=(pointsA[indo[1]*2+1]-cal[2])/cal[0];

        p[3]=pointsB[indo[1]*3];
        p[4]=pointsB[indo[1]*3+1];
        p[5]=pointsB[indo[1]*3+2];


        q3[0]=(pointsA[indo[2]*2]-cal[1])/cal[0];
        q3[1]=(pointsA[indo[2]*2+1]-cal[2])/cal[0];

        p[6]=pointsB[indo[2]*3];
        p[7]=pointsB[indo[2]*3+1];
        p[8]=pointsB[indo[2]*3+2];
    
        nq1=sqrt(q1[0]*q1[0]+q1[1]*q1[1]+1);
        nq2=sqrt(q2[0]*q2[0]+q2[1]*q2[1]+1);
        nq3=sqrt(q3[0]*q3[0]+q3[1]*q3[1]+1);

        q1[0]=q1[0]/nq1;
        q1[1]=q1[1]/nq1;
        q1[2]=1./nq1;
        q2[0]=q2[0]/nq2;
        q2[1]=q2[1]/nq2;
        q2[2]=1./nq2;
        q3[0]=q3[0]/nq3;
        q3[1]=q3[1]/nq3;
        q3[2]=1./nq3;

        p3p1(x,q1,q2,q3,p,p+3,p+6,&cnt);

        for(k=0;k<cnt;k++)
        {
            p3p2(x+k*3,q1,q2,q3,X,X+3,X+6);
            calc_p3pRt1(qt,Rt,tt,X,p);
            length=sqrt((tt[0]-ot[0])*(tt[0]-ot[0])+(tt[1]-ot[1])*(tt[1]-ot[1])+(tt[2]-ot[2])*(tt[2]-ot[2]));
            if((qt[0]*oq[0]+qt[1]*oq[1]+qt[2]*oq[2]+qt[3]*oq[3])>dq)
            {
                p3p2devt1(&dev,res,Rt,tt,cal,pointsA,pointsB,num_points,thres);
                dev+=(length-dt)*(length-dt)*scale;
                if(dev<(*devmin))
                {
                    q[0]=qt[0];
                    q[1]=qt[1];
                    q[2]=qt[2];
                    q[3]=qt[3];
                    t[0]=tt[0];
                    t[1]=tt[1];
                    t[2]=tt[2];
                    (*devmin)=dev;
                }
            }
        }
    }
}

void p3p1(double *s,double *q1,double *q2,double *q3,double *p1,double *p2,double *p3, int *num)
{
    double Rab,Rac,Rbc,tab,tac,tbc,K1,K2;
    double X[4],t,x[4],y,G0,G1,G2,G3,G4,p,pp,q,qp,a[5],a1[5],b[5];
    int k,cnt;
    

    Rab=(p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1])+(p1[2]-p2[2])*(p1[2]-p2[2]);
    Rac=(p1[0]-p3[0])*(p1[0]-p3[0])+(p1[1]-p3[1])*(p1[1]-p3[1])+(p1[2]-p3[2])*(p1[2]-p3[2]);
    Rbc=(p3[0]-p2[0])*(p3[0]-p2[0])+(p3[1]-p2[1])*(p3[1]-p2[1])+(p3[2]-p2[2])*(p3[2]-p2[2]);
    tab=((q1[0]*q2[0])+(q1[1]*q2[1])+(q1[2]*q2[2]));
    tac=((q1[0]*q3[0])+(q1[1]*q3[1])+(q1[2]*q3[2]));
    tbc=((q3[0]*q2[0])+(q3[1]*q2[1])+(q3[2]*q2[2]));
    K1=Rbc/Rac;
    K2=Rbc/Rab;

    G4=(K1*K2-K1-K2)*(K1*K2-K1-K2)-4*K1*K2*tbc*tbc;
    G3=4*(K1*K2-K1-K2)*K2*(1-K1)*tab+4*K1*tbc*((K1*K2+K2-K1)*tac+2*K2*tab*tbc);
    G2=2*K2*(1-K1)*tab*2*K2*(1-K1)*tab+2*(K1*K2+K1-K2)*(K1*K2-K1-K2)+4*K1*((K1-K2)*tbc*tbc+(1-K2)*K1*tac*tac-2*K2*(1+K1)*tab*tac*tbc);
    G1=4*(K1*K2+K1-K2)*K2*(1-K1)*tab+4*K1*((K1*K2-K1+K2)*tac*tbc+2*K1*K2*tab*tac*tac);
    G0=(K1*K2+K1-K2)*(K1*K2+K1-K2)-4*K1*K1*K2*tac*tac;

    X[0]=G3/G4;
    X[1]=G2/G4;
    X[2]=G1/G4;
    X[3]=G0/G4;
    /*find roots*/
    a[0]=1.;
    a[1]=X[0];
    a[2]=X[1];
    a[3]=X[2];
    a[4]=X[3];

    x[0]=0;
    x[1]=0;
    x[2]=0;
    x[3]=0;
    find_positive_real_roots(a,a1,b,x,5,50,&cnt);

   // printf("a=[ %f %f %f %f %f]\n",1.,X[0],X[1],X[2],X[3]);
  //   printf("count:%d\n",cnt);
    for(k=0;k<cnt;k++)
    {
        t=x[k];
 //       f0=t*t*t*t+X[0]*t*t*t+X[1]*t*t+X[2]*t+X[3];

        pp=2*(-t*tbc);
        p=2*(K1*tac-t*tbc);
        q=t*t-K1;
        qp=t*t*(1-K2)+2*t*K2*tab-K2;

        s[3*k]=sqrt(Rab)/(sqrt(t*t-2*t*tab+1));
        s[3*k+1]=s[3*k]*t;
        y=(pp*q-p*qp)/((1-K1)*qp-q);
        s[3*k+2]=y*s[3*k];
    //  printf("s%d %f %f %f\n",k,s[3*k],s[3*k+1],s[3*k+2]);    
    }   
    (*num)=cnt;

}

void calc_p3pRt(double *q,double *R,double *t, double *pointsA,double *pointsB)
{
    double U[512],V[64],S[9],X[64];
    int j,k,l;
    double pA[9],pB[9];
    double mq[3],mp[3];

    for(j=0;j<3;j++)
    {
        pA[j*3]=pointsA[j*3];
        pA[j*3+1]=pointsA[j*3+1];
        pA[j*3+2]=pointsA[j*3+2];
        pB[j*3]=pointsB[j*3];
        pB[j*3+1]=pointsB[j*3+1];
        pB[j*3+2]=pointsB[j*3+2];
    }
    mq[0]=0;
    mq[1]=0;
    mq[2]=0;
    mp[0]=0;
    mp[1]=0;
    mp[2]=0;
    for(j=0;j<3;j++)
    { 
        mq[0]+=pA[3*j];
        mq[1]+=pA[3*j+1];
        mq[2]+=pA[3*j+2];

    /* m4=mean(pts4)*/
        mp[0]+=pB[3*j];
        mp[1]+=pB[3*j+1];
        mp[2]+=pB[3*j+2];
    }
    mq[0]=mq[0]/3.;
    mq[1]=mq[1]/3.;
    mq[2]=mq[2]/3.;
    mp[0]=mp[0]/3.;
    mp[1]=mp[1]/3.;
    mp[2]=mp[2]/3.;
    for(j=0;j<3;j++)
    { 
        pA[3*j]-=mq[0];
        pA[3*j+1]-=mq[1];
        pA[3*j+2]-=mq[2];
        pB[3*j]-=mp[0];
        pB[3*j+1]-=mp[1];
        pB[3*j+2]-=mp[2];
    }

    calc_rot(q,X,pA,pB,3,V,U,S);
    quater_2_rot(R,q);
    
    for(k=0;k<3;k++)
    {
        t[k]=mq[k];
        for(l=0;l<3;l++)
            t[k]-=R[l*3+k]*mp[l];
    }
 
}

void calc_p3pRt1(double *q,double *R,double *t, double *pointsA,double *pointsB)
{
    double U[512],V[64],S[9],X[64];
    int j,k,l;
    double pA[9],pB[9];
    double mq[3],mp[3];

    for(j=0;j<3;j++)
    {
        pA[j*3]=pointsA[j*3];
        pA[j*3+1]=pointsA[j*3+1];
        pA[j*3+2]=pointsA[j*3+2];
        pB[j*3]=pointsB[j*3];
        pB[j*3+1]=pointsB[j*3+1];
        pB[j*3+2]=pointsB[j*3+2];
    }
    mq[0]=0;
    mq[1]=0;
    mq[2]=0;
    mp[0]=0;
    mp[1]=0;
    mp[2]=0;
    for(j=0;j<3;j++)
    { 
        mq[0]+=pA[3*j];
        mq[1]+=pA[3*j+1];
        mq[2]+=pA[3*j+2];

    /* m4=mean(pts4)*/
        mp[0]+=pB[3*j];
        mp[1]+=pB[3*j+1];
        mp[2]+=pB[3*j+2];
    }
    mq[0]=mq[0]/3.;
    mq[1]=mq[1]/3.;
    mq[2]=mq[2]/3.;
    mp[0]=mp[0]/3.;
    mp[1]=mp[1]/3.;
    mp[2]=mp[2]/3.;
    for(j=0;j<3;j++)
    { 
        pA[3*j]-=mq[0];
        pA[3*j+1]-=mq[1];
        pA[3*j+2]-=mq[2];
        pB[3*j]-=mp[0];
        pB[3*j+1]-=mp[1];
        pB[3*j+2]-=mp[2];
    }

    calc_rot(q,X,pA,pB,3,V,U,S);
    quater_2_rot(R,q);
    
    for(k=0;k<3;k++)
    {
        t[k]=-mp[k];
        for(l=0;l<3;l++)
            t[k]+=R[k*3+l]*mq[l];
    }
 
}

void calc_rot(double *q, double *X,double *pointsA,double  *pointsB, int num_points, double *V, double *U,double *S)
{
    int i,j;
    double mins,u[3],v[3];


    //Aj=[ 0       (u(1)-v(1)) (u(2)-v(2))  (u(3)-v(3))
    //(v(1)-u(1))   0           (-v(3)-u(3)) (v(2)+u(2))
    //(v(2)-u(2)) (v(3)+u(3))       0        (-v(1)-u(1))
    //(v(3)-u(3)) (-v(2)-u(2)) (v(1)+u(1))  0]
 
    for(i=0;i<num_points;i++){
        v[0]=pointsA[3*i];
        v[1]=pointsA[3*i+1];
        v[2]=pointsA[3*i+2];
        u[0]=pointsB[3*i];
        u[1]=pointsB[3*i+1];
        u[2]=pointsB[3*i+2];
        X[i*16]=0;
        X[1+i*16]=u[0]-v[0];
        X[2+i*16]=u[1]-v[1];
        X[3+i*16]=u[2]-v[2];
        X[4+i*16]=v[0]-u[0];
        X[5+i*16]=0;
        X[6+i*16]=-v[2]-u[2];
        X[7+i*16]=v[1]+u[1];
        X[8+i*16]=v[1]-u[1];
        X[9+i*16]=v[2]+u[2];
        X[10+i*16]=0;
        X[11+i*16]=-v[0]-u[0];
        X[12+i*16]=v[2]-u[2];
        X[13+i*16]=-u[1]-v[1];
        X[14+i*16]=v[0]+u[0];
        X[15+i*16]=0;
    }
    
    svduv(S,X,U,4*num_points,V,4);
          
    j=0;
    mins=S[j];

    for(i=1;i<4;i++)
    {
        if(mins>S[i])
        {
            j=i;
            mins=S[i];
        }
    }
      
    q[0]=V[j];
    q[1]=V[4*1+j];
    q[2]=V[4*2+j];
    q[3]=V[4*3+j];

}
void ransac_calc_rot(double *F, double *res, double *devmin, double *pointsA,double  *pointsB, int num_points)
{
    int i,j,k,incr;
    double norm,mins,dev,R[9],q[4],u[3],v[3],S[32],X[128],V[128],U[128];


    //Aj=[ 0       (u(1)-v(1)) (u(2)-v(2))  (u(3)-v(3))
    //(v(1)-u(1))   0           (-v(3)-u(3)) (v(2)+u(2))
    //(v(2)-u(2)) (v(3)+u(3))       0        (-v(1)-u(1))
    //(v(3)-u(3)) (-v(2)-u(2)) (v(1)+u(1))  0]

    *devmin=10000.0;
    for(i=0;i<100;i++)
    {
        j=-1;
        for(k=0;k<2;k++)
        {
            incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/2.);
            if(incr==0)
                incr=1;

            j=j+incr;
            norm=sqrt(pointsA[2*j]*pointsA[2*j]+pointsA[2*j+1]*pointsA[2*j+1]+1);
            v[0]=pointsA[2*j]/norm;
            v[1]=pointsA[2*j+1]/norm;
            v[2]=1./norm;
            norm=sqrt(pointsB[2*j]*pointsB[2*j]+pointsB[2*j+1]*pointsB[2*j+1]+1);
            u[0]=pointsB[2*j]/norm;
            u[1]=pointsB[2*j+1]/norm;
            u[2]=1./norm;
            X[k*16]=0;
            X[1+k*16]=u[0]-v[0];
            X[2+k*16]=u[1]-v[1];
            X[3+k*16]=u[2]-v[2];
            X[4+k*16]=v[0]-u[0];
            X[5+k*16]=0;
            X[6+k*16]=-v[2]-u[2];
            X[7+k*16]=v[1]+u[1];
            X[8+k*16]=v[1]-u[1];
            X[9+k*16]=v[2]+u[2];
            X[10+k*16]=0;
            X[11+k*16]=-v[0]-u[0];
            X[12+k*16]=v[2]-u[2];
            X[13+k*16]=-u[1]-v[1];
            X[14+k*16]=v[0]+u[0];
            X[15+k*16]=0;
        }

        svduv(S,X,U,8,V,4);
  
        
        j=0;
        mins=S[j];
        for(k=1;k<4;k++)
        {
            if(mins>S[k])
            {
                j=k;
                mins=S[k];
            }
        }
        
        q[0]=V[j];
        q[1]=V[4*1+j];
        q[2]=V[4*2+j];
        q[3]=V[4*3+j];

        quater_2_rot(R,q);
        calcdevh(&dev,res,R,pointsB,pointsA,num_points);
        if(dev<*devmin)
        {
           *devmin=dev;
            F[0]=q[0];
            F[1]=q[1];
            F[2]=q[2];
            F[3]=q[3];
        }
    }
}
void ransac_calc_rot_p(double *F, double *res, double *devmin, double *pointsA,double  *pointsB, double *p,int num_points)
{
    int i,j,k,incr;
    double norm,mins,dev,R[9],q[4],u[3],v[3],S[32],X[128],V[128],U[128],pA[2],pB[2];


    //Aj=[ 0       (u(1)-v(1)) (u(2)-v(2))  (u(3)-v(3))
    //(v(1)-u(1))   0           (-v(3)-u(3)) (v(2)+u(2))
    //(v(2)-u(2)) (v(3)+u(3))       0        (-v(1)-u(1))
    //(v(3)-u(3)) (-v(2)-u(2)) (v(1)+u(1))  0]

    *devmin=10000.0;
    for(i=0;i<100;i++)
    {
        j=-1;
        for(k=0;k<2;k++)
        {
            incr=(int)(((float)rand()/(float)RAND_MAX*(float)num_points)/2.);
            if(incr==0)
                incr=1;

            j=j+incr;
            pA[0]=(pointsA[2*j]-p[1])/p[0];
            pA[1]=(pointsA[2*j+1]-p[2])/p[0];
            norm=sqrt(pA[0]*pA[0]+pA[1]*pA[1]+1);
            v[0]=pA[0]/norm;
            v[1]=pA[1]/norm;
            v[2]=1./norm;
            pB[0]=(pointsB[2*j]-p[1])/p[0];
            pB[1]=(pointsB[2*j+1]-p[2])/p[0];
            norm=sqrt(pB[0]*pB[0]+pB[1]*pB[1]+1);
            u[0]=pB[0]/norm;
            u[1]=pB[1]/norm;
            u[2]=1./norm;
            X[k*16]=0;
            X[1+k*16]=u[0]-v[0];
            X[2+k*16]=u[1]-v[1];
            X[3+k*16]=u[2]-v[2];
            X[4+k*16]=v[0]-u[0];
            X[5+k*16]=0;
            X[6+k*16]=-v[2]-u[2];
            X[7+k*16]=v[1]+u[1];
            X[8+k*16]=v[1]-u[1];
            X[9+k*16]=v[2]+u[2];
            X[10+k*16]=0;
            X[11+k*16]=-v[0]-u[0];
            X[12+k*16]=v[2]-u[2];
            X[13+k*16]=-u[1]-v[1];
            X[14+k*16]=v[0]+u[0];
            X[15+k*16]=0;
        }

        svduv(S,X,U,8,V,4);
  
        
        j=0;
        mins=S[j];
        for(k=1;k<4;k++)
        {
            if(mins>S[k])
            {
                j=k;
                mins=S[k];
            }
        }
        
        q[0]=V[j];
        q[1]=V[4*1+j];
        q[2]=V[4*2+j];
        q[3]=V[4*3+j];

        quater_2_rot(R,q);
        calcdevp(&dev,res,R,pointsB,pointsA,num_points,p);
        if(dev<*devmin)
        {
           *devmin=dev;
            F[0]=q[0];
            F[1]=q[1];
            F[2]=q[2];
            F[3]=q[3];
        }
    }
}

void p3p2(double *x,double *q1,double *q2,double *q3,double *p1,double *p2,double *p3)
{

    p1[0]=x[0]*q1[0];
    p1[1]=x[0]*q1[1];
    p1[2]=x[0]*q1[2];
    p2[0]=x[1]*q2[0];
    p2[1]=x[1]*q2[1];
    p2[2]=x[1]*q2[2];
    p3[0]=x[2]*q3[0];
    p3[1]=x[2]*q3[1];
    p3[2]=x[2]*q3[2];	
}

void p3p2dev(double *dev,double *res, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int num_points)
{
    int i;
    double a,b,c,d1,d2;


    for(i=0;i<num_points;i++){
        a=R[0]*pointsB[3*i]+R[3]*pointsB[3*i+1]+R[6]*pointsB[3*i+2]+t[0];
        b=R[1]*pointsB[3*i]+R[4]*pointsB[3*i+1]+R[7]*pointsB[3*i+2]+t[1];
        c=R[2]*pointsB[3*i]+R[5]*pointsB[3*i+1]+R[8]*pointsB[3*i+2]+t[2];
        if(c>0)
        {
            d1=pointsA[2*i]-cal[1]-cal[0]*a/c;
            d2=pointsA[2*i+1]-cal[2]-cal[0]*b/c;
            res[i]=d1*d1+d2*d2;
        }
        else
            res[i]=1000;
    }
    jsort(res,num_points);
    *dev=res[(int)(num_points/2)+1];
}

void p3p2dev1(double *dev,double *res, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int num_points)
{
    int i;
    double a,b,c,d1,d2;


    for(i=0;i<num_points;i++){
        a=R[0]*(pointsB[3*i]+t[0])+R[3]*(pointsB[3*i+1]+t[1])+R[6]*(pointsB[3*i+2]+t[2]);
        b=R[1]*(pointsB[3*i]+t[0])+R[4]*(pointsB[3*i+1]+t[1])+R[7]*(pointsB[3*i+2]+t[2]);
        c=R[2]*(pointsB[3*i]+t[0])+R[5]*(pointsB[3*i+1]+t[1])+R[8]*(pointsB[3*i+2]+t[2]);
        if(c>0)
        {
            d1=pointsA[2*i]-cal[1]-cal[0]*a/c;
            d2=pointsA[2*i+1]-cal[2]-cal[0]*b/c;
            res[i]=d1*d1+d2*d2;
        }
        else
            res[i]=1000;
    }
    jsort(res,num_points);
    *dev=res[(int)(num_points/2)+1];
}

void p3p2devt(double *dev,double *res, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int numPoints,double thres)
{
    int i;
    double a,b,c,d1,d2,er,ssd;

    ssd=0;
    for(i=0;i<numPoints;i++){
        a=R[0]*pointsB[3*i]+R[3]*pointsB[3*i+1]+R[6]*pointsB[3*i+2]+t[0];
        b=R[1]*pointsB[3*i]+R[4]*pointsB[3*i+1]+R[7]*pointsB[3*i+2]+t[1];
        c=R[2]*pointsB[3*i]+R[5]*pointsB[3*i+1]+R[8]*pointsB[3*i+2]+t[2];
        
        if(c<=0)
        {
            er=thres;
        }
        else
        {
            d1=pointsA[2*i]-cal[1]-cal[0]*a/c;
            d2=pointsA[2*i+1]-cal[2]-cal[0]*b/c;
        
            er=d1*d1+d2*d2;
        }
        res[i]=er;
        if(er>thres)
            er=thres;
        if(c<=0)
            er=thres;
        ssd+=er;
    }
    
    *dev=ssd;
}
void p3p2devt1(double *dev,double *res, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int numPoints,double thres)
{
    int i;
    double a,b,c,d1,d2,er,ssd;

    ssd=0;
    for(i=0;i<numPoints;i++){
        a=R[0]*(pointsB[3*i]+t[0])+R[3]*(pointsB[3*i+1]+t[1])+R[6]*(pointsB[3*i+2]+t[2]);
        b=R[1]*(pointsB[3*i]+t[0])+R[4]*(pointsB[3*i+1]+t[1])+R[7]*(pointsB[3*i+2]+t[2]);
        c=R[2]*(pointsB[3*i]+t[0])+R[5]*(pointsB[3*i+1]+t[1])+R[8]*(pointsB[3*i+2]+t[2]);
        
        if(c<=0)
        {
            er=thres;
        }
        else
        {
            d1=pointsA[2*i]-cal[1]-cal[0]*a/c;
            d2=pointsA[2*i+1]-cal[2]-cal[0]*b/c;
        
            er=d1*d1+d2*d2;
        }
        res[i]=er;
        if(er>thres)
            er=thres;
        if(c<=0)
            er=thres;
        ssd+=er;
    }
    
    *dev=ssd;
}

void p3p2cnt(int *count, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int numPoints,double thres)
{
    int i,cnt;
    double a,b,c,d1,d2,er;

    cnt=0;
    for(i=0;i<numPoints;i++){
        a=R[0]*pointsB[3*i]+R[3]*pointsB[3*i+1]+R[6]*pointsB[3*i+2]+t[0];
        b=R[1]*pointsB[3*i]+R[4]*pointsB[3*i+1]+R[7]*pointsB[3*i+2]+t[1];
        c=R[2]*pointsB[3*i]+R[5]*pointsB[3*i+1]+R[8]*pointsB[3*i+2]+t[2];
       
        d1=pointsA[2*i]-cal[1]-cal[0]*a/c;
        d2=pointsA[2*i+1]-cal[2]-cal[0]*b/c;
        
        er=d1*d1+d2*d2;
        if(er<thres)
            cnt++;
    }
    
    *count=cnt;
}

void relative_transf(
                double *v1,
                double *v2,
                double *v3
                )
{
    double R[12];

/* relative rotation 2->1 v1x[v2(0) -v2(1:3)]*/
    v3[0]=v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]+v1[3]*v2[3];
    v3[1]=-v1[0]*v2[1]+v1[1]*v2[0] -v1[2]*v2[3]+v1[3]*v2[2];
    v3[2]=-v1[0]*v2[2]+v1[2]*v2[0] +v1[1]*v2[3]-v1[3]*v2[1];
    v3[3]=-v1[0]*v2[3]+v1[3]*v2[0] -v1[1]*v2[2]+v1[2]*v2[1];	
    
    quater_2_rot(R,v3);

    v3[4]=v1[4]-R[0]*v2[4]-R[3]*v2[5]-R[6]*v2[6];
    v3[5]=v1[5]-R[1]*v2[4]-R[4]*v2[5]-R[7]*v2[6];
    v3[6]=v1[6]-R[2]*v2[4]-R[5]*v2[5]-R[8]*v2[6];
}

void relative_transf1(
                double *v1,
                double *v2,
                double *v3
                )
{
    double R[12];

/* relative rotation 2->1 v1x[v2(0) -v2(1:3)]*/
    v3[0]=v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]+v1[3]*v2[3];
    v3[1]=-v1[0]*v2[1]+v1[1]*v2[0] -v1[2]*v2[3]+v1[3]*v2[2];
    v3[2]=-v1[0]*v2[2]+v1[2]*v2[0] +v1[1]*v2[3]-v1[3]*v2[1];
    v3[3]=-v1[0]*v2[3]+v1[3]*v2[0] -v1[1]*v2[2]+v1[2]*v2[1];    
    
    quater_2_rot(R,v3);

    v3[4]=v1[4]-v2[4];
    v3[5]=v1[5]-v2[5];
    v3[6]=v1[6]-v2[6];
}

void motion3D_update(
                double *v1,
                double *v2,
                double *v3
                )
{
    double R[12];
    
 /*   v3[0]=v1[0]*v2[0]-v1[1]*v2[1]-v1[2]*v2[2]-v1[3]*v2[3];
    v3[1]=v1[0]*v2[1]+v1[1]*v2[0]+v1[2]*v2[3]-v1[3]*v2[2];
    v3[2]=v1[0]*v2[2]+v1[2]*v2[0]-v1[1]*v2[3]+v1[3]*v2[1];
    v3[3]=v1[0]*v2[3]+v1[3]*v2[0]+v1[1]*v2[2]-v1[2]*v2[1];
*/
    v3[0]=v1[0]*v2[0]-v1[1]*v2[1]-v1[2]*v2[2]-v1[3]*v2[3];
    v3[1]=v1[0]*v2[1]+v1[1]*v2[0]-v1[2]*v2[3]+v1[3]*v2[2];
    v3[2]=v1[0]*v2[2]+v1[1]*v2[3]+v1[2]*v2[0]-v1[3]*v2[1];
    v3[3]=v1[0]*v2[3]-v1[1]*v2[2]+v1[2]*v2[1]+v1[3]*v2[0];
    

    quater_2_rot(R,v2);

    v3[4]=v2[4]+R[0]*v1[4]+R[3]*v1[5]+R[6]*v1[6];
    v3[5]=v2[5]+R[1]*v1[4]+R[4]*v1[5]+R[7]*v1[6];
    v3[6]=v2[6]+R[2]*v1[4]+R[5]*v1[5]+R[8]*v1[6];

}
void motion3D_update1(
                double *v1,
                double *v2,
                double *v3
                )
{
    
/*  v3[0]=v1[0]*v2[0]-v1[1]*v2[1]-v1[2]*v2[2]-v1[3]*v2[3];
    v3[1]=v1[0]*v2[1]+v1[1]*v2[0]+v1[2]*v2[3]-v1[3]*v2[2];
    v3[2]=v1[0]*v2[2]+v1[2]*v2[0]-v1[1]*v2[3]+v1[3]*v2[1];
    v3[3]=v1[0]*v2[3]+v1[3]*v2[0]+v1[1]*v2[2]-v1[2]*v2[1];
*/
    v3[0]=v1[0]*v2[0]-v1[1]*v2[1]-v1[2]*v2[2]-v1[3]*v2[3];
    v3[1]=v1[0]*v2[1]+v1[1]*v2[0]-v1[2]*v2[3]+v1[3]*v2[2];
    v3[2]=v1[0]*v2[2]+v1[1]*v2[3]+v1[2]*v2[0]-v1[3]*v2[1];
    v3[3]=v1[0]*v2[3]-v1[1]*v2[2]+v1[2]*v2[1]+v1[3]*v2[0];

    v3[4]=v1[4]+v2[4];
    v3[5]=v1[5]+v2[5];
    v3[6]=v1[6]+v2[6];

}
void inv_rigid_motion(double *vout, double *vin)
{
    double R[12],*q,*t,*q1,*t1;
    
    q=vin;
    t=vin+4;
    q1=vout;
    t1=vout+4;

    q1[0]=q[0];
    q1[1]=-q[1];
    q1[2]=-q[2];
    q1[3]=-q[3];
    
    quater_2_rot(R,q1);

    t1[0]=-R[0]*t[0]-R[3]*t[1]-R[6]*t[2];
    t1[1]=-R[1]*t[0]-R[4]*t[1]-R[7]*t[2];
    t1[2]=-R[2]*t[0]-R[5]*t[1]-R[8]*t[2];

}
void proj3D_err(
                double *er,
                double *v,
                double *pts,
                double *X,
                int *maski,
                double th,
                int *count,              
                int *kk,
                int N
                )
{
    double P[12],z,x,y,*p,e[2];

    int i=0,index,cnt,j;
    
    p=v+7;
    
    quater_2_rot(P,v);

    P[9]=v[4];
    P[10]=v[5];
    P[11]=v[6];

    cnt=0;

    j=0;
    for(i=0;i<N;i++)
    {
        index=maski[i];
        if(index>0)
        {
            index--;
             /* projecting object 3D-coordinates to camera frame */
            x=P[0]*X[3*index]+P[3]*X[1+3*index]+P[6]*X[2+3*index]+P[9];
            y=P[1]*X[3*index]+P[4]*X[1+3*index]+P[7]*X[2+3*index]+P[10];
            z=P[2]*X[3*index]+P[5]*X[1+3*index]+P[8]*X[2+3*index]+P[11];

            /* residuals */
            e[0]=pts[i*2]-p[1]-p[0]*x/z;
            e[1]=pts[1+i*2]-p[2]-p[0]*y/z;
            er[j]=e[0]*e[0]+e[1]*e[1];
        //    printf("er[%d]: %f\n",i,er[j]); 
            if(sqrt(er[j])>th)
            {
                maski[i]=-1;
            }
           else
               cnt++;
           j++;
        }
    }
    (*kk)=j;
    (*count)=cnt;
}

void proj3D_err1(
                double *er,
                double *v,
                double *pts,
                double *X,
                int *maski,
                double th,
                int *count,              
                int *kk,
                int N
                )
{
    double P[12],z,x,y,*p,e[2];

    int i=0,index,cnt,j;
    
    p=v+7;
    
    quater_2_rot(P,v);

    P[9]=v[4];
    P[10]=v[5];
    P[11]=v[6];

    cnt=0;

    j=0;
    for(i=0;i<N;i++)
    {
        index=maski[i];
        if(index>0)
        {
            index--;
             /* projecting object 3D-coordinates to camera frame */
            x=P[0]*(X[3*index]+P[9])+P[3]*(X[1+3*index]+P[10])+P[6]*(X[2+3*index]+P[11]);
            y=P[1]*(X[3*index]+P[9])+P[4]*(X[1+3*index]+P[10])+P[7]*(X[2+3*index]+P[11]);
            z=P[2]*(X[3*index]+P[9])+P[5]*(X[1+3*index]+P[10])+P[8]*(X[2+3*index]+P[11]);

            /* residuals */
            e[0]=pts[i*2]-p[1]-p[0]*x/z;
            e[1]=pts[1+i*2]-p[2]-p[0]*y/z;
            er[j]=e[0]*e[0]+e[1]*e[1];
        //    printf("er[%d]: %f\n",i,er[j]); 
            if(sqrt(er[j])>th)
            {
                maski[i]=-1;
            }
           else
               cnt++;
           j++;
        }
    }
    (*kk)=j;
    (*count)=cnt;
}

void move3D(
		double *v,
		double *X1,
		double *X2,		
		int N
	        )
{
    double P[12],norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,q[4],t[3];
    int i=0;
    
    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /*building Euclidian transformation matrix*/

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    
    for(i=0;i<N;i++)
    {
             /* projecting object 3D-coordinates to next frame */
            X2[3*i]=P[0]*X1[3*i]+P[3]*X1[1+3*i]+P[6]*X1[2+3*i]+P[9];
            X2[3*i+1]=P[1]*X1[3*i]+P[4]*X1[1+3*i]+P[7]*X1[2+3*i]+P[10];
            X2[3*i+2]=P[2]*X1[3*i]+P[5]*X1[1+3*i]+P[8]*X1[2+3*i]+P[11];
    }
}
void esse_funt(
              double *ssd,
              double *JJ,
              double *Je,
              double *v,
              double *pts1,
              double *pts2,
              double th,
              int N
              )
{
    double R[9],E[9],norm,num,den,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a,a1,b1,c1,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    double da1dR0,da1dR1,da1dR2,da1dt0,da1dt1,da1dt2,db1dt0,db1dt1,db1dt2,db1dR3,db1dR4,db1dR5,dc1dR6,dc1dR7,dc1dR8,dc1dt0,dc1dt1,dc1dt2;
 //   double da2dR1,da2dR2,da2dR4,da2dR5,da2dR7,da2dR8,da2dt1,da2dt2,db2dR0,db2dR2,db2dR3,db2dR5,db2dR6,db2dR8,db2dt0,db2dt2;
    double dp0dt0,dp0dt1,dp0dt2,dp1dt0,dp1dt1,dp1dt2,dp2dt0,dp2dt1,dp2dt2,dp[3],t[3],tnorm,J[7],e,er,x1,y1,x2,y2,q[4],to[3],p[3];
    int i=0,j,k;

    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    to[0]=v[4];
    to[1]=v[5];
    to[2]=v[6];
    p[0]=v[7];
    p[1]=v[8];
    p[2]=v[9];

    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1]; 
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    //building Euclidian transformation matrix

    R[0]=(q1t2+q2t2-q3t2-q4t2);
    R[1]=2*(q2q3+q1q4);
    R[2]=2*(q2q4-q1q3);
    R[3]=2*(q2q3-q1q4);
    R[4]=(q1t2-q2t2+q3t2-q4t2);
    R[5]=2*(q3q4+q1q2);
    R[6]=2*(q2q4+q1q3);
    R[7]=2*(q3q4-q1q2);
    R[8]=(q1t2-q2t2-q3t2+q4t2);


    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*R[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*R[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*R[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*R[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*R[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*R[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*R[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*R[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*R[8];
	
    dRdq1[0]= q[1]*norm*2-a*q[1]*R[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*R[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*R[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*R[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*R[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*R[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*R[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*R[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*R[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*R[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*R[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*R[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*R[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*R[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*R[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*R[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*R[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*R[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*R[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*R[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*R[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*R[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*R[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*R[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*R[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*R[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*R[8];
	
    R[0]*=norm;
    R[1]*=norm;
    R[2]*=norm;
    R[3]*=norm;
    R[4]*=norm;
    R[5]*=norm;
    R[6]*=norm;
    R[7]*=norm;
    R[8]*=norm;
        
    tnorm=1/sqrt(to[0]*to[0]+to[1]*to[1]+to[2]*to[2]);
    t[0]=to[0]*tnorm;
    t[1]=to[1]*tnorm;
    t[2]=to[2]*tnorm;
    dp0dt0=tnorm-to[0]*to[0]*tnorm*tnorm*tnorm;
    dp0dt1=-to[0]*to[1]*tnorm*tnorm*tnorm;
    dp0dt2=-to[0]*to[2]*tnorm*tnorm*tnorm;
    dp1dt0=-to[0]*to[1]*tnorm*tnorm*tnorm;
    dp1dt1=tnorm-to[1]*to[1]*tnorm*tnorm*tnorm;
    dp1dt2=-to[1]*to[2]*tnorm*tnorm*tnorm;
    dp2dt0=-to[2]*to[0]*tnorm*tnorm*tnorm;
    dp2dt1=-to[2]*to[1]*tnorm*tnorm*tnorm;
    dp2dt2=tnorm-to[2]*to[2]*tnorm*tnorm*tnorm;
    
    //skew(t)=[0 -t[2] t[1];t[2] 0 -t[0];-t[1] t[0] 0],E=skew(t)*R
    E[0]=t[1]*R[2]-t[2]*R[1];
    E[1]=t[2]*R[0]-t[0]*R[2];
    E[2]=t[0]*R[1]-t[1]*R[0];
    E[3]=t[1]*R[5]-t[2]*R[4];
    E[4]=t[2]*R[3]-t[0]*R[5];
    E[5]=t[0]*R[4]-t[1]*R[3];
    E[6]=t[1]*R[8]-t[2]*R[7];
    E[7]=t[2]*R[6]-t[0]*R[8];
    E[8]=t[0]*R[7]-t[1]*R[6];
    
    for(j=7-1;j>=0;j--)
    {
        for(k=j;k>=0;k--)
        {
            JJ[k+7*j]=0.;
        }
        Je[j]=0.;
    }
    er=0.;
    
    for(i=0;i<N;i++)//BOOGIE
    { 
        x2=(pts2[i*2]-p[1])/p[0];
        y2=(pts2[i*2+1]-p[2])/p[0];

        x1=(pts1[i*2]-p[1])/p[0];
        y1=(pts1[i*2+1]-p[2])/p[0];
        a1=E[0]*x1+E[1]*y1+E[2];    //pts1(:)'*E(:,1)
        b1=E[3]*x1+E[4]*y1+E[5];    //pts1(:)'*E(:,2)
        c1=E[6]*x1+E[7]*y1+E[8];    //pts1(:)'*E(:,2)

        num=1./(a1*a1+b1*b1);
        den=a1*x2+b1*y2+c1;
        e=den*sqrt(num);//(a1*a1+b1*b1);
	if(e>th)
		e=th;
    	if(e<-th)
	        e=-th;
        //j8=((2*f)*E[0]*pts1[i*2]*pts2[i*2]+(2*f)*E[1]*pts1[i*2+1]*pts2[i*2]+E[2]*pts2[i*2]
        //+(2*f)*E[3]*pts1[i*2]*pts2[i*2+1]+(2*f)*E[4]*pts1[i*2+1]*pts2[i*2+1]+E[5]*pts2[i*2+1]
        //+E[6]*pts1[i*2]+E[7]*pts1[i*2+1])*den-num*den*den*(
        da1dR0=y1*t[2]-t[1];
        da1dR1=t[0]-x1*t[2];
        da1dR2=x1*t[1]-y1*t[0];
        da1dt0=R[1]-y1*R[2];
        da1dt1=x1*R[2]-R[0];
        da1dt2=y1*R[0]-x1*R[1];
    
        db1dR3=da1dR0;//pts1[1]*t[2]-t[1];
        db1dR4=da1dR1;//t[0]-pts1[0]*t[2];
        db1dR5=da1dR2;//pts1[0]*t[1]-pts1[1]*t[0];
        db1dt0=R[4]-y1*R[5];
        db1dt1=x1*R[5]-R[3];
        db1dt2=y1*R[3]-x1*R[4];

        dc1dR6=da1dR0;//pts1[1]*t[2]-t[1];
        dc1dR7=da1dR1;//t[0]-pts1[0]*t[2];
        dc1dR8=da1dR2;//pts1[0]*t[1]-pts1[1]*t[0];
        dc1dt0=R[7]-y1*R[8];
        dc1dt1=x1*R[8]-R[6];
        dc1dt2=y1*R[6]-x1*R[7];
    /*
        da2dR1=-x2*t[2];
        da2dR2=x2*t[1];
        da2dR4=-y2*t[2];
        da2dR5=y2*t[1];
        da2dR7=-t[2];
        da2dR8=t[1];
        da2dt1=x2*R[2]+y2*R[5]+R[8];
        da2dt2=-x2*R[1]-y2*R[4]-R[7];
    
        db2dR0=-da2dR1;//pts2[0]*t[2];
        db2dR2=-x2*t[0];
        db2dR3=-da2dR4;//pts2[1]*t[2];
        db2dR5=-y2*t[0];
        db2dR6=t[2];
        db2dR8=-t[0];
        db2dt0=-da2dt1;//-pts2[i*2]*R[2]-pts2[i*2+1]*R[5]-R[8];
        db2dt2=x2*R[0]+y2*R[3]+R[6];*/
    
        J[0]=x2*(da1dR0*dRdq0[0]+da1dR1*dRdq0[1]+da1dR2*dRdq0[2])+y2*(db1dR3*dRdq0[3]+db1dR4*dRdq0[4]+db1dR5*dRdq0[5])+dc1dR6*dRdq0[6]+dc1dR7*dRdq0[7]+dc1dR8*dRdq0[8];
        J[1]=x2*(da1dR0*dRdq1[0]+da1dR1*dRdq1[1]+da1dR2*dRdq1[2])+y2*(db1dR3*dRdq1[3]+db1dR4*dRdq1[4]+db1dR5*dRdq1[5])+dc1dR6*dRdq1[6]+dc1dR7*dRdq1[7]+dc1dR8*dRdq1[8];
        J[2]=x2*(da1dR0*dRdq2[0]+da1dR1*dRdq2[1]+da1dR2*dRdq2[2])+y2*(db1dR3*dRdq2[3]+db1dR4*dRdq2[4]+db1dR5*dRdq2[5])+dc1dR6*dRdq2[6]+dc1dR7*dRdq2[7]+dc1dR8*dRdq2[8];
        J[3]=x2*(da1dR0*dRdq3[0]+da1dR1*dRdq3[1]+da1dR2*dRdq3[2])+y2*(db1dR3*dRdq3[3]+db1dR4*dRdq3[4]+db1dR5*dRdq3[5])+dc1dR6*dRdq3[6]+dc1dR7*dRdq3[7]+dc1dR8*dRdq3[8];
        dp[0]=x2*da1dt0+y2*db1dt0+dc1dt0;
        dp[1]=x2*da1dt1+y2*db1dt1+dc1dt1;
        dp[2]=x2*da1dt2+y2*db1dt2+dc1dt2;

        J[0]*=sqrt(num);
        J[1]*=sqrt(num);
        J[2]*=sqrt(num);
        J[3]*=sqrt(num);
        dp[0]*=sqrt(num);
        dp[1]*=sqrt(num);
        dp[2]*=sqrt(num);
        
        J[0]-=sqrt(num)*num*den*(a1*(da1dR0*dRdq0[0]+da1dR1*dRdq0[1]+da1dR2*dRdq0[2])
                +b1*(db1dR3*dRdq0[3]+db1dR4*dRdq0[4]+db1dR5*dRdq0[5]));
    
        J[1]-=sqrt(num)*num*den*(a1*(da1dR0*dRdq1[0]+da1dR1*dRdq1[1]+da1dR2*dRdq1[2])
                +b1*(db1dR3*dRdq1[3]+db1dR4*dRdq1[4]+db1dR5*dRdq1[5]));
    
        J[2]-=sqrt(num)*num*den*(a1*(da1dR0*dRdq2[0]+da1dR1*dRdq2[1]+da1dR2*dRdq2[2])
                +b1*(db1dR3*dRdq2[3]+db1dR4*dRdq2[4]+db1dR5*dRdq2[5]));
    
        J[3]-=sqrt(num)*num*den*(a1*(da1dR0*dRdq3[0]+da1dR1*dRdq3[1]+da1dR2*dRdq3[2])
                +b1*(db1dR3*dRdq3[3]+db1dR4*dRdq3[4]+db1dR5*dRdq3[5]));

        dp[0]-=sqrt(num)*num*den*(a1*da1dt0+b1*db1dt0);
    
        dp[1]-=sqrt(num)*num*den*(a1*da1dt1+b1*db1dt1);
    
        dp[2]-=sqrt(num)*num*den*(a1*da1dt2+b1*db1dt2);
        
        J[4]=dp[0]*dp0dt0+dp[1]*dp1dt0+dp[2]*dp2dt0;
        J[5]=dp[0]*dp0dt1+dp[1]*dp1dt1+dp[2]*dp2dt1;
        J[6]=dp[0]*dp0dt2+dp[1]*dp1dt2+dp[2]*dp2dt2;
        for(j=7-1;j>=0;j--)
        {
            for(k=j;k>=0;k--)
            {
                JJ[k+7*j]+=J[j]*J[k];
            }
            Je[j]+=J[j]*e;
        }
	er+=e*e;
    }
    (*ssd)=er;
}
void esse_calct(double *v,double *pts1,double *pts2,double *lambda, double th, int N)
{
    double  JJ[49], Je[7],L[49],g[7],tol,ssd,ssdmin,vmin[10],norm;
    int j,n,D=7;

    norm=1./sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
    vmin[0]=v[0]*norm;
    vmin[1]=v[1]*norm;
    vmin[2]=v[2]*norm;
    vmin[3]=v[3]*norm;
    vmin[4]=v[4];
    vmin[5]=v[5];
    vmin[6]=v[6];
 
    esse_funt(&ssdmin,JJ,Je,v,pts1,pts2,th,N);
    for(j=0;j<D;j++)
        JJ[j+j*D]+=(*lambda);
   
    choldc(L,JJ,D);
    cholsl(L,Je,g,D);
    for(j=0;j<D;j++)v[j]=v[j]-g[j];
    for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
  //  printf("ssdmin:%f\n",ssdmin);
    for(n=1;n<20;n++)
    {
        esse_funt(&ssd,JJ,Je,v,pts1,pts2,th,N);

    //    printf("ssd:%f\n",ssd);
        if(ssd<ssdmin)
        {
     //       printf("ssdmin:%f\n",ssd);
            ssdmin=ssd;
            norm=1./sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
            vmin[0]=v[0]*norm;
            vmin[1]=v[1]*norm;
            vmin[2]=v[2]*norm;
            vmin[3]=v[3]*norm;
	   
            vmin[4]=v[4];
            vmin[5]=v[5];
            vmin[6]=v[6];
            (*lambda)/=10.;
        }
        else
        {
            v[0]=vmin[0];
            v[1]=vmin[1];
            v[2]=vmin[2];
            v[3]=vmin[3];
            v[4]=vmin[4];
            v[5]=vmin[5];
            v[6]=vmin[6];
            (*lambda)*=9.;
            esse_funt(&ssd,JJ,Je,v,pts1,pts2,th,N);
        }
        if(n<19)
	{
        for(j=0;j<D;j++)
            JJ[j+j*D]+=(*lambda);
     //   printf("Je: %f %f JJ: %f %f \n",Je[0],Je[1],JJ[0],JJ[D]);
        if(choldc(L,JJ,D)<0)
        {
            printf("esse_calc choldc fails");
        }
        cholsl(L,Je,g,D);
        for(j=0;j<D;j++)v[j]=v[j]-g[j];
        for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
        }
    }
    v[0]=vmin[0];
    v[1]=vmin[1];
    v[2]=vmin[2];
    v[3]=vmin[3];
    v[4]=vmin[4];
    v[5]=vmin[5];
    v[6]=vmin[6];
}
void proj3D_funt(
                double *ssd,
                double *JJ,
                double *Je,
                double *v,
                double *pts,
                double *X,
                int *mask,
                double th,
                int N
        )
{
    double J[16],P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a,q[4],t[3],p[3],e[2],er;
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    int i=0,j,k,D=7,index;
    
    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];
    p[0]=v[7];
    p[1]=v[8];
    p[2]=v[9];
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /*building Euclidian transformation matrix*/

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];

    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];

    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    
    for(j=D-1;j>=0;j--)
    {
        for(k=j;k>=0;k--)
        {
            JJ[k+D*j]=0.;
        }
        Je[j]=0.;
    }
    er=0.;
    
    for(i=0;i<N;i++)
    {
        index=mask[i];
        if(index>0)
        {
            index--;
            /* projecting object 3D-coordinates to camera frame */
            x=P[0]*X[3*index]+P[3]*X[1+3*index]+P[6]*X[2+3*index]+P[9];
            y=P[1]*X[3*index]+P[4]*X[1+3*index]+P[7]*X[2+3*index]+P[10];
            z=P[2]*X[3*index]+P[5]*X[1+3*index]+P[8]*X[2+3*index]+P[11];

            /* residuals */
            e[0]=pts[i*2]-p[1]-p[0]*x/z;
            e[1]=pts[1+i*2]-p[2]-p[0]*y/z;
            if(e[0]>th)
                e[0]=th;
            if(e[0]<-th)
                e[0]=-th;
            if(e[1]>th)
                e[1]=th;
            if(e[1]<-th)
                e[1]=-th;	
            
	    er+=e[0]*e[0]+e[1]*e[1];
	    
            z2=z*z;
            f1=-p[0]*x/z2;
            f2=-p[0]*y/z2;
            dudP0=p[0]*X[3*index]/z;
            dudP3=p[0]*X[1+3*index]/z;
            dudP6=p[0]*X[2+3*index]/z;
            dudP2=f1*X[3*index];
            dudP5=f1*X[1+3*index];
            dudP8=f1*X[2+3*index];

            dvdP1=p[0]*X[3*index]/z;
            dvdP4=p[0]*X[1+3*index]/z;
            dvdP7=p[0]*X[2+3*index]/z;
            dvdP2=f2*X[3*index];
            dvdP5=f2*X[1+3*index];
            dvdP8=f2*X[2+3*index];
        
            /* Jacobian */
            /* motion parameters for u */
            J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
            J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
            J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
            J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
            J[4]=p[0]/z;      /* t1 */
            J[5]=0.;          /* t2 */
            J[6]=f1;          /* t3 */

            /* motion parameters for v */
            J[7]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
            J[8]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
            J[9]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
            J[10]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
            J[11]=0.;          /* t1 */
            J[12]=p[0]/z;      /* t2 */
            J[13]=f2;          /* t3 */
            
 
            for(j=D-1;j>=0;j--)
            {
                for(k=j;k>=0;k--)
                {
                    JJ[k+D*j]+=J[j+D]*J[k+D];
                }
                Je[j]+=J[j+D]*e[1];
            }
            for(j=D-1;j>=0;j--)
            {
                for(k=j;k>=0;k--)
                {
                    JJ[k+D*j]+=J[j]*J[k];
                }
                Je[j]+=J[j]*e[0];
            }
        
        }
    }
    (*ssd)=er;
}
void proj3D_funt1(
                double *ssd,
                double *JJ,
                double *Je,
                double *v,
                const double *pts,
                const double *X,
                const double th,
                const int N
        )
{
    double J[16],P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a,q[4],t[3],p[3],e[2],er;
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    int i=0,j,k,D=7;
    
    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];
    p[0]=v[7];
    p[1]=v[8];
    p[2]=v[9];
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /*building Euclidian transformation matrix*/

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];

    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];

    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    
    for(j=D-1;j>=0;j--)
    {
        for(k=j;k>=0;k--)
        {
            JJ[k+D*j]=0.;
        }
        Je[j]=0.;
    }
    er=0.;
    
    for(i=0;i<N;i++)
    {
            /* projecting object 3D-coordinates to camera frame */
        x=P[0]*X[3*i]+P[3]*X[1+3*i]+P[6]*X[2+3*i]+P[9];
        y=P[1]*X[3*i]+P[4]*X[1+3*i]+P[7]*X[2+3*i]+P[10];
        z=P[2]*X[3*i]+P[5]*X[1+3*i]+P[8]*X[2+3*i]+P[11];

            /* residuals */
        e[0]=pts[i*2]-p[1]-p[0]*x/z;
        e[1]=pts[1+i*2]-p[2]-p[0]*y/z;
        if(e[0]>th)
            e[0]=th;
        if(e[0]<-th)
            e[0]=-th;
        if(e[1]>th)
            e[1]=th;
        if(e[1]<-th)
            e[1]=-th;   
            
        er+=e[0]*e[0]+e[1]*e[1];
        
        z2=z*z;
        f1=-p[0]*x/z2;
        f2=-p[0]*y/z2;
        dudP0=p[0]*X[3*i]/z;
        dudP3=p[0]*X[1+3*i]/z;
        dudP6=p[0]*X[2+3*i]/z;
        dudP2=f1*X[3*i];
        dudP5=f1*X[1+3*i];
        dudP8=f1*X[2+3*i];

        dvdP1=p[0]*X[3*i]/z;
        dvdP4=p[0]*X[1+3*i]/z;
        dvdP7=p[0]*X[2+3*i]/z;
        dvdP2=f2*X[3*i];
        dvdP5=f2*X[1+3*i];
        dvdP8=f2*X[2+3*i];
        
            /* Jacobian */
            /* motion parameters for u */
        J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
        J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
        J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
        J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
        J[4]=p[0]/z;      /* t1 */
        J[5]=0.;          /* t2 */
        J[6]=f1;          /* t3 */

            /* motion parameters for v */
        J[7]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
        J[8]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
        J[9]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
        J[10]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
        J[11]=0.;          /* t1 */
        J[12]=p[0]/z;      /* t2 */
        J[13]=f2;          /* t3 */
            
        for(j=D-1;j>=0;j--)
        {
            for(k=j;k>=0;k--)
            {
                JJ[k+D*j]+=J[j+D]*J[k+D];
            }
            Je[j]+=J[j+D]*e[1];
        }
        for(j=D-1;j>=0;j--)
        {
            for(k=j;k>=0;k--)
            {
                JJ[k+D*j]+=J[j]*J[k];
            }
            Je[j]+=J[j]*e[0];
        }
    }
    (*ssd)=er;
}
void proj3D_funtl1(
                double *ssd,
                double *JJ,
                double *Je,
                double *v,
                double *v0,
                double r,
                double scale,
                const double *pts,
                const double *X,
                const double th,
                const int N
        )
{
    double J[16],P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a,q[4],t[3],p[3],e[2],er;
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    double ssum,rhat,dx,dy,dz;
    int i=0,j,k,D=7;
    
    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];
    p[0]=v[7];
    p[1]=v[8];
    p[2]=v[9];
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /*building Euclidian transformation matrix*/

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];

    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];

    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    
    for(j=D-1;j>=0;j--)
    {
        for(k=j;k>=0;k--)
        {
            JJ[k+D*j]=0.;
        }
        Je[j]=0.;
    }
    er=0.;
    
    for(i=0;i<N;i++)
    {
            /* projecting object 3D-coordinates to camera frame */
        x=P[0]*(X[3*i]+P[9])+P[3]*(X[1+3*i]+P[10])+P[6]*(X[2+3*i]+P[11]);
        y=P[1]*(X[3*i]+P[9])+P[4]*(X[1+3*i]+P[10])+P[7]*(X[2+3*i]+P[11]);
        z=P[2]*(X[3*i]+P[9])+P[5]*(X[1+3*i]+P[10])+P[8]*(X[2+3*i]+P[11]);

            /* residuals */
        e[0]=pts[i*2]-p[1]-p[0]*x/z;
        e[1]=pts[1+i*2]-p[2]-p[0]*y/z;
        if(e[0]>th)
            e[0]=th;
        if(e[0]<-th)
            e[0]=-th;
        if(e[1]>th)
            e[1]=th;
        if(e[1]<-th)
            e[1]=-th;   
            
        er+=e[0]*e[0]+e[1]*e[1];
            
        z2=z*z;
        f1=-p[0]*x/z2;
        f2=-p[0]*y/z2;
        dudP0=p[0]*(X[3*i]+P[9])/z;
        dudP3=p[0]*(X[1+3*i]+P[10])/z;
        dudP6=p[0]*(X[2+3*i]+P[11])/z;
        dudP2=f1*(X[3*i]+P[9]);
        dudP5=f1*(X[1+3*i]+P[10]);
        dudP8=f1*(X[2+3*i]+P[11]);
        
        dvdP1=p[0]*(X[3*i]+P[9])/z;
        dvdP4=p[0]*(X[1+3*i]+P[10])/z;
        dvdP7=p[0]*(X[2+3*i]+P[11])/z;
        dvdP2=f2*(X[3*i]+P[9]);
        dvdP5=f2*(X[1+3*i]+P[10]);
        dvdP8=f2*(X[2+3*i]+P[11]);
        
    /* Jacobian */
        
        /* motion parameters for u */
        J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
        J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
        J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
        J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
        J[4]=p[0]*P[0]/z+f1*P[2];       //t1
        J[5]=p[0]*P[3]/z+f1*P[5];           //t2
        J[6]=p[0]*P[6]/z+f1*P[8];           //t3


            /* motion parameters for v */
        J[D]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
        J[D+1]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
        J[D+2]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
        J[D+3]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
        J[D+4]=p[0]*P[1]/z+f2*P[2];           /* t1 */
        J[D+5]=p[0]*P[4]/z+f2*P[5];       /* t2 */
        J[D+6]=p[0]*P[7]/z+f2*P[8];           /* t3 */
            
        for(j=D-1;j>=0;j--)
        {
            for(k=j;k>=0;k--)
            {
                JJ[k+D*j]+=J[j+D]*J[k+D];
            }
            Je[j]+=J[j+D]*e[1];
        }
        for(j=D-1;j>=0;j--)
        {
            for(k=j;k>=0;k--)
            {
                JJ[k+D*j]+=J[j]*J[k];
            }
            Je[j]+=J[j]*e[0];
        }
    }
    dx=v[4]-v0[4];
    dy=v[5]-v0[5];
    dz=v[6]-v0[6];
    
    ssum=dx*dx+dy*dy+dz*dz;
    rhat=sqrt(ssum);
    e[0]=r-rhat;
    
    /*Jacobian*/
    J[0]=dx/rhat;
    J[1]=dy/rhat;
    J[2]=dz/rhat;
    for(j=D-1;j>=4;j--)
    {
        for(k=j;k>=4;k--)
        {
            JJ[k+D*j]+=J[j]*J[k]*scale;
        }
        Je[j]+=J[j]*e[0]*scale;
    }
    (*ssd)=er;
}

void pose_calct(double *v,double *pts1,double *pts2,int *mask,double *lambda, double th, int N)
{
    double  JJ[49], Je[7],L[49],g[7],tol,ssd,ssdmin,vmin[10],norm;   
    int j,n,D=7;

    vmin[0]=v[0];
    vmin[1]=v[1];
    vmin[2]=v[2];
    vmin[3]=v[3];
    vmin[4]=v[4];
    vmin[5]=v[5];
    vmin[6]=v[6];
 
    proj3D_funt(&ssdmin,JJ,Je,v,pts1,pts2,mask,th,N);
    for(j=0;j<D;j++)
        JJ[j+j*D]+=(*lambda);
   
    choldc(L,JJ,D);
    cholsl(L,Je,g,D);
    for(j=0;j<D;j++)v[j]=v[j]+g[j];
    for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
    
    for(n=1;n<20;n++)
    {
        proj3D_funt(&ssd,JJ,Je,v,pts1,pts2,mask,th,N);

   
        if(ssd<ssdmin)
        {
  //          printf("ssd:%f\n",ssd);    
            ssdmin=ssd;
            norm=1./sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
            vmin[0]=v[0]*norm;
            vmin[1]=v[1]*norm;
            vmin[2]=v[2]*norm;
            vmin[3]=v[3]*norm;
            vmin[4]=v[4];
            vmin[5]=v[5];
            vmin[6]=v[6];
            (*lambda)/=10.;
        }
        else
        {
            v[0]=vmin[0];
            v[1]=vmin[1];
            v[2]=vmin[2];
            v[3]=vmin[3];
            v[4]=vmin[4];
            v[5]=vmin[5];
            v[6]=vmin[6];
            (*lambda)*=9.;
            proj3D_funt(&ssd,JJ,Je,v,pts1,pts2,mask,th,N);
        }
        if(n<19)
	{
        for(j=0;j<D;j++)
            JJ[j+j*D]+=(*lambda);
     //   printf("Je: %f %f JJ: %f %f \n",Je[0],Je[1],JJ[0],JJ[D]);
        if(choldc(L,JJ,D)<0)
        {
            printf("pose_calc choldc fails");
        }
        cholsl(L,Je,g,D);
        for(j=0;j<D;j++)v[j]=v[j]+g[j];
        for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
        }
    }
    v[0]=vmin[0];
    v[1]=vmin[1];
    v[2]=vmin[2];
    v[3]=vmin[3];
    v[4]=vmin[4];
    v[5]=vmin[5];
    v[6]=vmin[6];
}

void pose_calct1(double *v,const double *pts1,const double *pts2,double *lambda,const double th,const int N)
{
    double  JJ[49], Je[7],L[49],g[7],tol,ssd,ssdmin=1000.,vmin[10],norm;  
    int j,n,D=7;

    vmin[0]=v[0];
    vmin[1]=v[1];
    vmin[2]=v[2];
    vmin[3]=v[3];
    vmin[4]=v[4];
    vmin[5]=v[5];
    vmin[6]=v[6];
    if(N==0)
      printf("pose_calct1 error: N=0\n");
    proj3D_funt1(&ssdmin,JJ,Je,v,pts1,pts2,th,N);
    for(j=0;j<D;j++)
        JJ[j+j*D]+=(*lambda);
   
    choldc(L,JJ,D);
    cholsl(L,Je,g,D);
    for(j=0;j<D;j++)v[j]=v[j]+g[j];
    for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
    
    for(n=1;n<20;n++)
    {
        proj3D_funt1(&ssd,JJ,Je,v,pts1,pts2,th,N);

   
        if(ssd<ssdmin)
        {
  //          printf("ssd:%f\n",ssd);    
            ssdmin=ssd;
            norm=1./sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
            vmin[0]=v[0]*norm;
            vmin[1]=v[1]*norm;
            vmin[2]=v[2]*norm;
            vmin[3]=v[3]*norm;
            vmin[4]=v[4];
            vmin[5]=v[5];
            vmin[6]=v[6];
            (*lambda)/=10.;
        }
        else
        {
            v[0]=vmin[0];
            v[1]=vmin[1];
            v[2]=vmin[2];
            v[3]=vmin[3];
            v[4]=vmin[4];
            v[5]=vmin[5];
            v[6]=vmin[6];
            (*lambda)*=9.;
            proj3D_funt1(&ssd,JJ,Je,v,pts1,pts2,th,N);
        }
        if(n<19)
    {
        for(j=0;j<D;j++)
            JJ[j+j*D]+=(*lambda);
     //   printf("Je: %f %f JJ: %f %f \n",Je[0],Je[1],JJ[0],JJ[D]);
        if(choldc(L,JJ,D)<0)
        {
            printf("pose_calc choldc fails");
        }
        cholsl(L,Je,g,D);
        for(j=0;j<D;j++)v[j]=v[j]+g[j];
        for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
        }
    }
    v[0]=vmin[0];
    v[1]=vmin[1];
    v[2]=vmin[2];
    v[3]=vmin[3];
    v[4]=vmin[4];
    v[5]=vmin[5];
    v[6]=vmin[6];
}

void pose_calctl1(double *v,double *v0,double r,double S,const double *pts1,const double *pts2,double *lambda,const double th,const int N)
{
    double  JJ[49], Je[7],L[49],g[7],tol,ssd,ssdmin=1000.,vmin[10],norm;  
    int j,n,D=7;

    vmin[0]=v[0];
    vmin[1]=v[1];
    vmin[2]=v[2];
    vmin[3]=v[3];
    vmin[4]=v[4];
    vmin[5]=v[5];
    vmin[6]=v[6];
    
    proj3D_funtl1(&ssdmin,JJ,Je,v,v0,r,S,pts1,pts2,th,N);
    for(j=0;j<D;j++)
        JJ[j+j*D]+=(*lambda);
   
    choldc(L,JJ,D);
    cholsl(L,Je,g,D);
    for(j=0;j<D;j++)v[j]=v[j]+g[j];
    for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
    
    for(n=1;n<20;n++)
    {
        proj3D_funtl1(&ssd,JJ,Je,v,v0,r,S,pts1,pts2,th,N);

   
        if(ssd<ssdmin)
        {
  //          printf("ssd:%f\n",ssd);    
            ssdmin=ssd;
            norm=1./sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
            vmin[0]=v[0]*norm;
            vmin[1]=v[1]*norm;
            vmin[2]=v[2]*norm;
            vmin[3]=v[3]*norm;
            vmin[4]=v[4];
            vmin[5]=v[5];
            vmin[6]=v[6];
            (*lambda)/=10.;
        }
        else
        {
            v[0]=vmin[0];
            v[1]=vmin[1];
            v[2]=vmin[2];
            v[3]=vmin[3];
            v[4]=vmin[4];
            v[5]=vmin[5];
            v[6]=vmin[6];
            (*lambda)*=9.;
            proj3D_funtl1(&ssd,JJ,Je,v,v0,r,S,pts1,pts2,th,N);
        }
        if(n<19)
    {
        for(j=0;j<D;j++)
            JJ[j+j*D]+=(*lambda);
     //   printf("Je: %f %f JJ: %f %f \n",Je[0],Je[1],JJ[0],JJ[D]);
        if(choldc(L,JJ,D)<0)
        {
            printf("pose_calc choldc fails");
        }
        cholsl(L,Je,g,D);
        for(j=0;j<D;j++)v[j]=v[j]+g[j];
        for(tol=0.,j=0;j<D;j++)tol+=fabs(g[j]);
        }
    }
    v[0]=vmin[0];
    v[1]=vmin[1];
    v[2]=vmin[2];
    v[3]=vmin[3];
    v[4]=vmin[4];
    v[5]=vmin[5];
    v[6]=vmin[6];
}

void triangulate2(
              double *X,
              double *Xpos,
              double *v,
              double *pts1,
              double *pts2,
              int *ind2,
              int *indX,
              int *mask,
              double th,
              int *kk,
              int N)
{

    double J[16],P1[12],P2[12],V[16],U[16],S[4],er;
    double E[9],R[9],t[3],num,den,a1,a2,b1,b2,c1,pts11,pts12,pts21,pts22,e,mins,q1[4],q2[4],t1[3],t2[3],x1,y1,x2,y2,p[3];
    int i=0,j,k,n;
    
    
    q1[0]=Xpos[0];
    q1[1]=Xpos[1];
    q1[2]=Xpos[2];
    q1[3]=Xpos[3];
    t1[0]=Xpos[4];
    t1[1]=Xpos[5];
    t1[2]=Xpos[6];
    
    q2[0]=v[0];
    q2[1]=v[1];
    q2[2]=v[2];
    q2[3]=v[3];
    t2[0]=v[4];
    t2[1]=v[5];
    t2[2]=v[6];
    p[0]=v[7];
    p[1]=v[8];
    p[2]=v[9];
    quater_2_rot(P1,q1);
   
    P1[9]=t1[0];
    P1[10]=t1[1];
    P1[11]=t1[2];

    quater_2_rot(P2,q2);
    
    P2[9]=t2[0];
    P2[10]=t2[1];
    P2[11]=t2[2];

    /*
    R=Rr*Rl'
    t=tr-Rr*Rl'*tl
    */
   
    R[0]=P2[0]*P1[0]+P2[3]*P1[3]+P2[6]*P1[6];
    R[1]=P2[1]*P1[0]+P2[4]*P1[3]+P2[7]*P1[6];
    R[2]=P2[2]*P1[0]+P2[5]*P1[3]+P2[8]*P1[6];
    R[3]=P2[0]*P1[1]+P2[3]*P1[4]+P2[6]*P1[7];
    R[4]=P2[1]*P1[1]+P2[4]*P1[4]+P2[7]*P1[7];
    R[5]=P2[2]*P1[1]+P2[5]*P1[4]+P2[8]*P1[7];
    R[6]=P2[0]*P1[2]+P2[3]*P1[5]+P2[6]*P1[8];
    R[7]=P2[1]*P1[2]+P2[4]*P1[5]+P2[7]*P1[8];
    R[8]=P2[2]*P1[2]+P2[5]*P1[5]+P2[8]*P1[8];
    
    t[0]=t2[0]-(R[0]*t1[0]+R[3]*t1[1]+R[6]*t1[2]);
    t[1]=t2[1]-(R[1]*t1[0]+R[4]*t1[1]+R[7]*t1[2]);
    t[2]=t2[2]-(R[2]*t1[0]+R[5]*t1[1]+R[8]*t1[2]);
 //   printf("t2: %f %f %f\n",t[0],t[1],t[2]);
    E[0]=t[1]*R[2]-t[2]*R[1];
    E[1]=t[2]*R[0]-t[0]*R[2];
    E[2]=t[0]*R[1]-t[1]*R[0];
    E[3]=t[1]*R[5]-t[2]*R[4];
    E[4]=t[2]*R[3]-t[0]*R[5];
    E[5]=t[0]*R[4]-t[1]*R[3];
    E[6]=t[1]*R[8]-t[2]*R[7];
    E[7]=t[2]*R[6]-t[0]*R[8];
    E[8]=t[0]*R[7]-t[1]*R[6];
    //printf("E: %f %f %f  %f %f %f  %f %f %f\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
    n=0;
    for(i=0;i<N;i++)
    {
        if(mask[i]==0)
        {
            mask[i]=-1;
            x2=(pts2[i*2]-p[1])/p[0];
            y2=(pts2[i*2+1]-p[2])/p[0];
            a2=E[0]*x2+E[1]*y2+E[2];    //E(1,:)*pts2(:)
            b2=E[3]*x2+E[4]*y2+E[5];    //E(2,:)*pts2(:)
            //c2=E[2]*pts2[i*2]+E[5]*pts2[i*2+1]+E[8];    //E(3,:)*pts2(:)
            x1=(pts1[i*2]-p[1])/p[0];
            y1=(pts1[i*2+1]-p[2])/p[0];
            a1=E[0]*x1+E[3]*y1+E[6];    //pts1(:)'*E(:,1)
            b1=E[1]*x1+E[4]*y1+E[7];    //pts1(:)'*E(:,2)
            c1=E[2]*x1+E[5]*y1+E[8];    //pts1(:)'*E(:,2)
            num=1/(a1*a1+b1*b1+a2*a2+b2*b2);
            den=a1*x2+b1*y2+c1;
            e=den*num;//(a1*a1+b1*b1+a2*a2+b2*b2);
            er=den/sqrt(a1*a1+b1*b1);
            if(fabs(p[0]*er)<th)
            {
                pts11=x1-e*a2;
                pts12=y1-e*b2;
                pts21=x2-e*a1;
                pts22=y2-e*b1;
        
                J[0] = P1[2] * pts11 - P1[0];
                J[4] = P1[5] * pts11 - P1[3];
                J[8] = P1[8] * pts11 - P1[6];
                J[12] = P1[11] * pts11 - P1[9];
    
                J[1] = P1[2] * pts12 - P1[1];
                J[5] = P1[5] * pts12 - P1[4];
                J[9] = P1[8] * pts12 - P1[7];
                J[13] = P1[11] * pts12 - P1[10];
    
                J[2] = P2[2] * pts21 - P2[0];
                J[6] = P2[5] * pts21 - P2[3];
                J[10] = P2[8] * pts21 - P2[6];
                J[14] = P2[11] * pts21 - P2[9];
        
                J[3] = P2[2] * pts22 - P2[1];
                J[7] = P2[5] * pts22 - P2[4];
                J[11] = P2[8] * pts22 - P2[7];
                J[15] = P2[11] * pts22 - P2[10];
        
                svduv(S,J,U,4,V,4);
                j=0;
                mins=S[j];
       
                for(k=1;k<4;k++)
                {
                    if(mins>S[k])
                    {
                        j=k;
                        mins=S[k];
                    }
                }
                if(fabs(U[12+j])>0.0000000001)
                {
                    if(U[12]<0)
                        U[12]=-0.0000000001;
                    else
                        U[12]=0.0000000001;
                }
                X[3*n]=U[j]/U[12+j];
                X[3*n+1]=U[4+j]/U[12+j];
                X[3*n+2]=U[8+j]/U[12+j];
 
                mask[i]=0;
                indX[n]=ind2[i];
                n++;
            }
        }
    }
    (*kk)=n;
}
void robust_triangulate1(
              double *X,
              double *Xpos,
              double *v,
              double *pts1,
              double *pts2,
              int *ind2,
              int *indX,
              int *mask,
              double th,
              int *kk,
              int N)
{

    double J[16],P1[12],P2[12],V[16],U[16],S[4],er,e1,e2;
    double E[9],R[9],t[3],num,den,a1,a2,b1,b2,c1,pts11,pts12,pts21,pts22,e,mins,q1[4],q2[4],t1[3],t2[3],x1,y1,x2,y2,p[3];
    int i=0,j,k,n;
    
    
    q1[0]=Xpos[0];
    q1[1]=Xpos[1];
    q1[2]=Xpos[2];
    q1[3]=Xpos[3];
    quater_2_rot(P1,q1);
    t1[0]=P1[0]*Xpos[4]+P1[3]*Xpos[5]+P1[6]*Xpos[6];
    t1[1]=P1[1]*Xpos[4]+P1[4]*Xpos[5]+P1[7]*Xpos[6];
    t1[2]=P1[2]*Xpos[4]+P1[5]*Xpos[5]+P1[8]*Xpos[6];
    
    q2[0]=v[0];
    q2[1]=v[1];
    q2[2]=v[2];
    q2[3]=v[3];
    quater_2_rot(P2,q2);
    t2[0]=P2[0]*v[4]+P2[3]*v[5]+P2[6]*v[6];
    t2[1]=P2[1]*v[4]+P2[4]*v[5]+P2[7]*v[6];
    t2[2]=P2[2]*v[4]+P2[5]*v[5]+P2[8]*v[6];

    p[0]=v[7];
    p[1]=v[8];
    p[2]=v[9];
 
   
    P1[9]=t1[0];
    P1[10]=t1[1];
    P1[11]=t1[2];


    
    P2[9]=t2[0];
    P2[10]=t2[1];
    P2[11]=t2[2];

    /*
    R=Rr*Rl'
    t=tr-Rr*Rl'*tl
    */
   
    R[0]=P2[0]*P1[0]+P2[3]*P1[3]+P2[6]*P1[6];
    R[1]=P2[1]*P1[0]+P2[4]*P1[3]+P2[7]*P1[6];
    R[2]=P2[2]*P1[0]+P2[5]*P1[3]+P2[8]*P1[6];
    R[3]=P2[0]*P1[1]+P2[3]*P1[4]+P2[6]*P1[7];
    R[4]=P2[1]*P1[1]+P2[4]*P1[4]+P2[7]*P1[7];
    R[5]=P2[2]*P1[1]+P2[5]*P1[4]+P2[8]*P1[7];
    R[6]=P2[0]*P1[2]+P2[3]*P1[5]+P2[6]*P1[8];
    R[7]=P2[1]*P1[2]+P2[4]*P1[5]+P2[7]*P1[8];
    R[8]=P2[2]*P1[2]+P2[5]*P1[5]+P2[8]*P1[8];
    
    t[0]=t2[0]-(R[0]*t1[0]+R[3]*t1[1]+R[6]*t1[2]);
    t[1]=t2[1]-(R[1]*t1[0]+R[4]*t1[1]+R[7]*t1[2]);
    t[2]=t2[2]-(R[2]*t1[0]+R[5]*t1[1]+R[8]*t1[2]);
 //   printf("t2: %f %f %f\n",t[0],t[1],t[2]);
    E[0]=t[1]*R[2]-t[2]*R[1];
    E[1]=t[2]*R[0]-t[0]*R[2];
    E[2]=t[0]*R[1]-t[1]*R[0];
    E[3]=t[1]*R[5]-t[2]*R[4];
    E[4]=t[2]*R[3]-t[0]*R[5];
    E[5]=t[0]*R[4]-t[1]*R[3];
    E[6]=t[1]*R[8]-t[2]*R[7];
    E[7]=t[2]*R[6]-t[0]*R[8];
    E[8]=t[0]*R[7]-t[1]*R[6];
    //printf("E: %f %f %f  %f %f %f  %f %f %f\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
    n=0;
    for(i=0;i<N;i++)
    {
        if(mask[i]==0)
        {
            mask[i]=-1;
            
            x2=(pts2[i*2]-p[1])/p[0];
            y2=(pts2[i*2+1]-p[2])/p[0];
            a2=E[0]*x2+E[1]*y2+E[2];    //E(1,:)*pts2(:)
            b2=E[3]*x2+E[4]*y2+E[5];    //E(2,:)*pts2(:)
            //c2=E[2]*pts2[i*2]+E[5]*pts2[i*2+1]+E[8];    //E(3,:)*pts2(:)
 
            x1=(pts1[i*2]-p[1])/p[0];
            y1=(pts1[i*2+1]-p[2])/p[0];
            a1=E[0]*x1+E[3]*y1+E[6];    //pts1(:)'*E(:,1)
            b1=E[1]*x1+E[4]*y1+E[7];    //pts1(:)'*E(:,2)
            c1=E[2]*x1+E[5]*y1+E[8];    //pts1(:)'*E(:,2)
            
            num=1/(a1*a1+b1*b1+a2*a2+b2*b2);
            den=a1*x2+b1*y2+c1;
            e=den*num;//(a1*a1+b1*b1+a2*a2+b2*b2);
            er=den/sqrt(a1*a1+b1*b1);
  //         printf("robuster: %f %d\n",p[0]*er,n);
            if(fabs(p[0]*er)<th)
            {
                pts11=x1-e*a2;
                pts12=y1-e*b2;
                pts21=x2-e*a1;
                pts22=y2-e*b1;
        
                J[0] = P1[2] * pts11 - P1[0];
                J[4] = P1[5] * pts11 - P1[3];
                J[8] = P1[8] * pts11 - P1[6];
                J[12] = P1[11] * pts11 - P1[9];
    
                J[1] = P1[2] * pts12 - P1[1];
                J[5] = P1[5] * pts12 - P1[4];
                J[9] = P1[8] * pts12 - P1[7];
                J[13] = P1[11] * pts12 - P1[10];
    
                J[2] = P2[2] * pts21 - P2[0];
                J[6] = P2[5] * pts21 - P2[3];
                J[10] = P2[8] * pts21 - P2[6];
                J[14] = P2[11] * pts21 - P2[9];
        
                J[3] = P2[2] * pts22 - P2[1];
                J[7] = P2[5] * pts22 - P2[4];
                J[11] = P2[8] * pts22 - P2[7];
                J[15] = P2[11] * pts22 - P2[10];
        
                svduv(S,J,U,4,V,4);
                j=0;
                mins=S[j];
       
                for(k=1;k<4;k++)
                {
                    if(mins>S[k])
                    {
                        j=k;
                        mins=S[k];
                    }
                }
                if(fabs(U[12+j])>0.0000000001)
                {
                    if(U[12]<0)
                        U[12]=-0.0000000001;
                    else
                        U[12]=0.0000000001;
                }
                X[3*n]=U[j]/U[12+j];
                X[3*n+1]=U[4+j]/U[12+j];
                X[3*n+2]=U[8+j]/U[12+j];
                a1=R[0]*x1+R[3]*y1+R[6];
                b1=R[1]*x1+R[4]*y1+R[7];
                c1=R[2]*x1+R[5]*y1+R[8];
                
                
                e1=(x2-a1/c1);
                e2=(y2-b1/c1);
                er=sqrt(e1*e1+e2*e2);
                mask[i]=0;
                if((p[0]*er)<th)
                {
                    c1=P1[2]*X[3*n]+P1[5]*X[3*n+1]+P1[8]*X[3*n+2]+P1[11];
                    if(c1<0)
                    {  
                      printf("depth negative: %d\n",i); 
                       X[3*n]=pts11*1000;
                       X[3*n+1]=pts12*1000;
                       X[3*n+2]=1000;
                    }
 
                }
                indX[n]=ind2[i];
                n++;
            }
        }
    }
    (*kk)=n;
}
void robust_triangulate2(
              double *X,
              double *Xpos,
              double *v,
              double *p1,
              double *p2,
              double *pts1,
              double *pts2,
              int *ind2,
              int *indX,
              int *mask,
              double th,
              int *kk,
              int N)
{

    double J[16],P1[12],P2[12],V[16],U[16],S[4],er,e1,e2;
    double E[9],R[9],t[3],num,den,a1,a2,b1,b2,c1,pts11,pts12,pts21,pts22,e,mins,q1[4],q2[4],t1[3],t2[3],x1,y1,x2,y2;
    int i=0,j,k,n;
    
    
    q1[0]=Xpos[0];
    q1[1]=Xpos[1];
    q1[2]=Xpos[2];
    q1[3]=Xpos[3];
    t1[0]=Xpos[4];
    t1[1]=Xpos[5];
    t1[2]=Xpos[6];
    
    q2[0]=v[0];
    q2[1]=v[1];
    q2[2]=v[2];
    q2[3]=v[3];
    t2[0]=v[4];
    t2[1]=v[5];
    t2[2]=v[6];

    quater_2_rot(P1,q1);
   
    P1[9]=t1[0];
    P1[10]=t1[1];
    P1[11]=t1[2];

    quater_2_rot(P2,q2);
    
    P2[9]=t2[0];
    P2[10]=t2[1];
    P2[11]=t2[2];

    /*
    R=Rr*Rl'
    t=tr-Rr*Rl'*tl
    */
   
    R[0]=P2[0]*P1[0]+P2[3]*P1[3]+P2[6]*P1[6];
    R[1]=P2[1]*P1[0]+P2[4]*P1[3]+P2[7]*P1[6];
    R[2]=P2[2]*P1[0]+P2[5]*P1[3]+P2[8]*P1[6];
    R[3]=P2[0]*P1[1]+P2[3]*P1[4]+P2[6]*P1[7];
    R[4]=P2[1]*P1[1]+P2[4]*P1[4]+P2[7]*P1[7];
    R[5]=P2[2]*P1[1]+P2[5]*P1[4]+P2[8]*P1[7];
    R[6]=P2[0]*P1[2]+P2[3]*P1[5]+P2[6]*P1[8];
    R[7]=P2[1]*P1[2]+P2[4]*P1[5]+P2[7]*P1[8];
    R[8]=P2[2]*P1[2]+P2[5]*P1[5]+P2[8]*P1[8];
    
    t[0]=t2[0]-(R[0]*t1[0]+R[3]*t1[1]+R[6]*t1[2]);
    t[1]=t2[1]-(R[1]*t1[0]+R[4]*t1[1]+R[7]*t1[2]);
    t[2]=t2[2]-(R[2]*t1[0]+R[5]*t1[1]+R[8]*t1[2]);
 //   printf("t2: %f %f %f\n",t[0],t[1],t[2]);
    E[0]=t[1]*R[2]-t[2]*R[1];
    E[1]=t[2]*R[0]-t[0]*R[2];
    E[2]=t[0]*R[1]-t[1]*R[0];
    E[3]=t[1]*R[5]-t[2]*R[4];
    E[4]=t[2]*R[3]-t[0]*R[5];
    E[5]=t[0]*R[4]-t[1]*R[3];
    E[6]=t[1]*R[8]-t[2]*R[7];
    E[7]=t[2]*R[6]-t[0]*R[8];
    E[8]=t[0]*R[7]-t[1]*R[6];
    printf("E: %f %f %f  %f %f %f  %f %f %f\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
    n=0;
    for(i=0;i<N;i++)
    {
        if(mask[i]==0)
        {
            mask[i]=-1;
            
            x2=(pts2[i*2]-p2[1])/p2[0];
            y2=(pts2[i*2+1]-p2[2])/p2[0];
            a2=E[0]*x2+E[1]*y2+E[2];    //E(1,:)*pts2(:)
            b2=E[3]*x2+E[4]*y2+E[5];    //E(2,:)*pts2(:)
            //c2=E[2]*pts2[i*2]+E[5]*pts2[i*2+1]+E[8];    //E(3,:)*pts2(:)
 
            x1=(pts1[i*2]-p1[1])/p1[0];
            y1=(pts1[i*2+1]-p1[2])/p1[0];
            a1=E[0]*x1+E[3]*y1+E[6];    //pts1(:)'*E(:,1)
            b1=E[1]*x1+E[4]*y1+E[7];    //pts1(:)'*E(:,2)
            c1=E[2]*x1+E[5]*y1+E[8];    //pts1(:)'*E(:,2)
            
            num=1/(a1*a1+b1*b1+a2*a2+b2*b2);
            den=a1*x2+b1*y2+c1;
            e=den*num;//(a1*a1+b1*b1+a2*a2+b2*b2);
            er=den/sqrt(a1*a1+b1*b1);
  //          printf("robuster: %f %d\n",p2[0]*er,n);
            if(fabs(p2[0]*er)<th)
            {
                pts11=x1-e*a2;
                pts12=y1-e*b2;
                pts21=x2-e*a1;
                pts22=y2-e*b1;
        
                J[0] = P1[2] * pts11 - P1[0];
                J[4] = P1[5] * pts11 - P1[3];
                J[8] = P1[8] * pts11 - P1[6];
                J[12] = P1[11] * pts11 - P1[9];
    
                J[1] = P1[2] * pts12 - P1[1];
                J[5] = P1[5] * pts12 - P1[4];
                J[9] = P1[8] * pts12 - P1[7];
                J[13] = P1[11] * pts12 - P1[10];
    
                J[2] = P2[2] * pts21 - P2[0];
                J[6] = P2[5] * pts21 - P2[3];
                J[10] = P2[8] * pts21 - P2[6];
                J[14] = P2[11] * pts21 - P2[9];
        
                J[3] = P2[2] * pts22 - P2[1];
                J[7] = P2[5] * pts22 - P2[4];
                J[11] = P2[8] * pts22 - P2[7];
                J[15] = P2[11] * pts22 - P2[10];
        
                svduv(S,J,U,4,V,4);
                j=0;
                mins=S[j];
       
                for(k=1;k<4;k++)
                {
                    if(mins>S[k])
                    {
                        j=k;
                        mins=S[k];
                    }
                }
                if(fabs(U[12+j])<0.0000000001)
                {
                    if(U[12+j]<0)
                        U[12+j]=-0.0000000001;
                    else
                        U[12+j]=0.0000000001;
                }
                X[3*n]=U[j]/U[12+j];
                X[3*n+1]=U[4+j]/U[12+j];
                X[3*n+2]=U[8+j]/U[12+j];

                c1=P1[2]*X[3*n]+P1[5]*X[3*n+1]+P1[8]*X[3*n+2]+P1[11];
               
                if(c1>0)
                {
                    if(ind2[i]<0)
                    {
                      printf("error: robust_triangulate2 ind2[%d] is %d\n",i,ind2[i]);
                      exit(1);
                    }
                    mask[i]=0;
                    indX[n]=ind2[i];
                    n++;
                }
                else
                {
                    printf("depth negative: %d\n",i); 
                    a1=R[0]*x1+R[3]*y1+R[6];
                    b1=R[1]*x1+R[4]*y1+R[7];
                    c1=R[2]*x1+R[5]*y1+R[8];
                    e1=(x2-a1/c1);
                    e2=(y2-b1/c1);
                    er=sqrt(e1*e1+e2*e2);
                    if((p2[0]*er)<th)
                    {
                       X[3*n]=pts11*1000;
                       X[3*n+1]=pts12*1000;
                       X[3*n+2]=1000;
                       mask[i]=0;
                       if(ind2[i]<0)
                    {
                      printf("error: robust_triangulate2 ind2[%d] is %d\n",i,ind2[i]);
                      exit(1);
                    }
                       indX[n]=ind2[i];
                       n++;
                    }
 
                }
            }
        }
    }
    (*kk)=n;
}

void robust_triangulate(
              double *X,
              double *Xpos,
              double *v,
              double *pts1,
              double *pts2,
              int *ind2,
              int *indX,
              int *mask,
              double th,
              int *kk,
              int N)
{

    double J[16],P1[12],P2[12],V[16],U[16],S[4],er,e1,e2;
    double E[9],R[9],t[3],num,den,a1,a2,b1,b2,c1,pts11,pts12,pts21,pts22,e,mins,q1[4],q2[4],t1[3],t2[3],x1,y1,x2,y2,p[3];
    int i=0,j,k,n;
    
    
    q1[0]=Xpos[0];
    q1[1]=Xpos[1];
    q1[2]=Xpos[2];
    q1[3]=Xpos[3];
    t1[0]=Xpos[4];
    t1[1]=Xpos[5];
    t1[2]=Xpos[6];
    
    q2[0]=v[0];
    q2[1]=v[1];
    q2[2]=v[2];
    q2[3]=v[3];
    t2[0]=v[4];
    t2[1]=v[5];
    t2[2]=v[6];
    p[0]=v[7];
    p[1]=v[8];
    p[2]=v[9];
    quater_2_rot(P1,q1);
   
    P1[9]=t1[0];
    P1[10]=t1[1];
    P1[11]=t1[2];

    quater_2_rot(P2,q2);
    
    P2[9]=t2[0];
    P2[10]=t2[1];
    P2[11]=t2[2];

    /*
    R=Rr*Rl'
    t=tr-Rr*Rl'*tl
    */
   
    R[0]=P2[0]*P1[0]+P2[3]*P1[3]+P2[6]*P1[6];
    R[1]=P2[1]*P1[0]+P2[4]*P1[3]+P2[7]*P1[6];
    R[2]=P2[2]*P1[0]+P2[5]*P1[3]+P2[8]*P1[6];
    R[3]=P2[0]*P1[1]+P2[3]*P1[4]+P2[6]*P1[7];
    R[4]=P2[1]*P1[1]+P2[4]*P1[4]+P2[7]*P1[7];
    R[5]=P2[2]*P1[1]+P2[5]*P1[4]+P2[8]*P1[7];
    R[6]=P2[0]*P1[2]+P2[3]*P1[5]+P2[6]*P1[8];
    R[7]=P2[1]*P1[2]+P2[4]*P1[5]+P2[7]*P1[8];
    R[8]=P2[2]*P1[2]+P2[5]*P1[5]+P2[8]*P1[8];
    
    t[0]=t2[0]-(R[0]*t1[0]+R[3]*t1[1]+R[6]*t1[2]);
    t[1]=t2[1]-(R[1]*t1[0]+R[4]*t1[1]+R[7]*t1[2]);
    t[2]=t2[2]-(R[2]*t1[0]+R[5]*t1[1]+R[8]*t1[2]);
 //   printf("t2: %f %f %f\n",t[0],t[1],t[2]);
    E[0]=t[1]*R[2]-t[2]*R[1];
    E[1]=t[2]*R[0]-t[0]*R[2];
    E[2]=t[0]*R[1]-t[1]*R[0];
    E[3]=t[1]*R[5]-t[2]*R[4];
    E[4]=t[2]*R[3]-t[0]*R[5];
    E[5]=t[0]*R[4]-t[1]*R[3];
    E[6]=t[1]*R[8]-t[2]*R[7];
    E[7]=t[2]*R[6]-t[0]*R[8];
    E[8]=t[0]*R[7]-t[1]*R[6];
  //  printf("E: %f %f %f  %f %f %f  %f %f %f\n",E[0],E[1],E[2],E[3],E[4],E[5],E[6],E[7],E[8]);
    n=0;
    for(i=0;i<N;i++)
    {
        if(mask[i]==0)
        {
            mask[i]=-1;
            
            x2=(pts2[i*2]-p[1])/p[0];
            y2=(pts2[i*2+1]-p[2])/p[0];
            a2=E[0]*x2+E[1]*y2+E[2];    //E(1,:)*pts2(:)
            b2=E[3]*x2+E[4]*y2+E[5];    //E(2,:)*pts2(:)
            //c2=E[2]*pts2[i*2]+E[5]*pts2[i*2+1]+E[8];    //E(3,:)*pts2(:)
 
            x1=(pts1[i*2]-p[1])/p[0];
            y1=(pts1[i*2+1]-p[2])/p[0];
            a1=E[0]*x1+E[3]*y1+E[6];    //pts1(:)'*E(:,1)
            b1=E[1]*x1+E[4]*y1+E[7];    //pts1(:)'*E(:,2)
            c1=E[2]*x1+E[5]*y1+E[8];    //pts1(:)'*E(:,2)
            
            num=1/(a1*a1+b1*b1+a2*a2+b2*b2);
            den=a1*x2+b1*y2+c1;
            e=den*num;//(a1*a1+b1*b1+a2*a2+b2*b2);
            er=den/sqrt(a1*a1+b1*b1);
         //  printf("er: %d %f\n",i,p[0]*er);
            if(fabs(p[0]*er)<th)
            {
                pts11=x1-e*a2;
                pts12=y1-e*b2;
                pts21=x2-e*a1;
                pts22=y2-e*b1;
        
                J[0] = P1[2] * pts11 - P1[0];
                J[4] = P1[5] * pts11 - P1[3];
                J[8] = P1[8] * pts11 - P1[6];
                J[12] = P1[11] * pts11 - P1[9];
    
                J[1] = P1[2] * pts12 - P1[1];
                J[5] = P1[5] * pts12 - P1[4];
                J[9] = P1[8] * pts12 - P1[7];
                J[13] = P1[11] * pts12 - P1[10];
    
                J[2] = P2[2] * pts21 - P2[0];
                J[6] = P2[5] * pts21 - P2[3];
                J[10] = P2[8] * pts21 - P2[6];
                J[14] = P2[11] * pts21 - P2[9];
        
                J[3] = P2[2] * pts22 - P2[1];
                J[7] = P2[5] * pts22 - P2[4];
                J[11] = P2[8] * pts22 - P2[7];
                J[15] = P2[11] * pts22 - P2[10];
        
                svduv(S,J,U,4,V,4);
                j=0;
                mins=S[j];
       
                for(k=1;k<4;k++)
                {
                    if(mins>S[k])
                    {
                        j=k;
                        mins=S[k];
                    }
                }
                if(fabs(U[12+j])<0.0000000001)
                {
                    if(U[12+j]<0)
                        U[12+j]=-0.0000000001;
                    else
                        U[12+j]=0.0000000001;
                }
                X[3*n]=U[j]/U[12+j];
                X[3*n+1]=U[4+j]/U[12+j];
                X[3*n+2]=U[8+j]/U[12+j];

                c1=P1[2]*X[3*n]+P1[5]*X[3*n+1]+P1[8]*X[3*n+2]+P1[11];
               // printf("c1: %d %f\n",i,c1);
                if(c1>0.)
                {
                    mask[i]=0;
                    indX[n]=ind2[i];
                    n++;
                }
                else
                {
                    a1=R[0]*x1+R[3]*y1+R[6];
                    b1=R[1]*x1+R[4]*y1+R[7];
                    c1=R[2]*x1+R[5]*y1+R[8];
                    e1=(x2-a1/c1);
                    e2=(y2-b1/c1);
                    er=sqrt(e1*e1+e2*e2);
                    if((p[0]*er)<th)
                    {
                       X[3*n]=pts11*1000;
                       X[3*n+1]=pts12*1000;
                       X[3*n+2]=1000;
                       mask[i]=0;
                       indX[n]=ind2[i];
                       n++;
                    }
                }
            }
        }
    }
    (*kk)=n;
}

void triangulate(
              double *X,
              double *v,
              double *pts1,
              double *pts2,
              double th,
              int N
              )
{

    double J[16],P[12],P1[12],V[16],U[16],S[4];
    double E[9],num,den,a1,a2,b1,b2,c1,pts11,pts12,pts21,pts22,e,mins,q[4],t[3],x1,y1,x2,y2;
    int i=0,j,k;

    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];

    quater_2_rot(P,q);
   
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    P1[0]=1.;
    P1[1]=0.;
    P1[2]=0.;
    P1[3]=0.;
    P1[4]=1.;
    P1[5]=0.;
    P1[6]=0.;
    P1[7]=0.;
    P1[8]=1.;
    P1[9]=0.;
    P1[10]=0.;
    P1[11]=0.;
    
    E[0]=t[1]*P[2]-t[2]*P[1];
    E[1]=t[2]*P[0]-t[0]*P[2];
    E[2]=t[0]*P[1]-t[1]*P[0];
    E[3]=t[1]*P[5]-t[2]*P[4];
    E[4]=t[2]*P[3]-t[0]*P[5];
    E[5]=t[0]*P[4]-t[1]*P[3];
    E[6]=t[1]*P[8]-t[2]*P[7];
    E[7]=t[2]*P[6]-t[0]*P[8];
    E[8]=t[0]*P[7]-t[1]*P[6];
    
    for(i=0;i<N;i++)
    {
        x2=pts2[i*2];
        y2=pts2[i*2+1];
        a2=E[0]*x2+E[3]*y2+E[6];    //E(1,:)*pts2(:)
        b2=E[1]*x2+E[4]*y2+E[7];    //E(2,:)*pts2(:)
        //c2=E[2]*pts2[i*2]+E[5]*pts2[i*2+1]+E[8];    //E(3,:)*pts2(:)
        x1=pts1[i*2];
        y1=pts1[i*2+1];
        a1=E[0]*x1+E[1]*y1+E[2];    //pts1(:)'*E(:,1)
        b1=E[3]*x1+E[4]*y1+E[5];    //pts1(:)'*E(:,2)
        c1=E[6]*x1+E[7]*y1+E[8];    //pts1(:)'*E(:,2)
        num=1/(a1*a1+b1*b1+a2*a2+b2*b2);
        den=a1*x2+b1*y2+c1;
        e=den*num;//(a1*a1+b1*b1+a2*a2+b2*b2);

        pts11=x1-e*a2;
        pts12=y1-e*b2;
        pts21=x2-e*a1;
        pts22=y2-e*b1;
        
        J[0] = P[2] * pts11 - P[0];
        J[4] = P[5] * pts11 - P[3];
        J[8] = P[8] * pts11 - P[6];
        J[12] = P[11] * pts11 - P[9];
    
        J[1] = P[2] * pts12 - P[1];
        J[5] = P[5] * pts12 - P[4];
        J[9] = P[8] * pts12 - P[7];
        J[13] = P[11] * pts12 - P[10];
   
        J[2] = P1[2] * pts21 - P1[0];
        J[6] = P1[5] * pts21 - P1[3];
        J[10] = P1[8] * pts21 - P1[6];
        J[14] = P1[11] * pts21 - P1[9];
        
        J[3] = P1[2] * pts22 - P1[1];
        J[7] = P1[5] * pts22 - P1[4];
        J[11] = P1[8] * pts22 - P1[7];
        J[15] = P1[11] * pts22 - P1[10];
        
        svduv(S,J,U,4,V,4);
        j=0;
        mins=S[j];

        for(k=1;k<4;k++)
        {
            if(mins>S[k])
            {
                j=k;
                mins=S[k];
            }
        }
            
        if(fabs(U[12+j])>0.0000000001)
        {
            if(U[12]<0)
                 U[12]=-0.0000000001;
            else
                 U[12]=0.0000000001;
        }
        X[3*i]=U[j]/U[12+j];
        X[3*i+1]=U[4+j]/U[12+j];
        X[3*i+2]=U[8+j]/U[12+j];

    }
}


void proj3D(
        double *v,
        double *p,
        double *pts,
        double *X,
        int N
              )
{
    double P[12],z,x,y,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,q[4],t[3];
    int i=0;
    
    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /*building Euclidian transformation matrix*/

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];


    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    
    for(i=0;i<N;i++)
    {

        /* projecting object 3D-coordinates to camera frame */
        x=P[0]*X[3*i]+P[3]*X[1+3*i]+P[6]*X[2+3*i]+P[9];
        y=P[1]*X[3*i]+P[4]*X[1+3*i]+P[7]*X[2+3*i]+P[10];
        z=P[2]*X[3*i]+P[5]*X[1+3*i]+P[8]*X[2+3*i]+P[11];

        /* residuals */
        pts[i*3]=p[1]+p[0]*x/z;
        pts[1+i*3]=p[2]+p[0]*y/z;
        pts[2+i*3]=z;
    

    }
}
void v2H(double *P, double *v,double *n, double *p)
{
    double H[9],norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,q[4],t[3],K[9],iK[9];
    int i,j,k;

    q[0]=v[0];
    q[1]=v[1];
    q[2]=v[2];
    q[3]=v[3];
    t[0]=v[4];
    t[1]=v[5];
    t[2]=v[6];
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /*building Euclidian transformation matrix*/

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;

    P[0]+=t[0]*n[0];
    P[1]+=t[1]*n[0];
    P[2]+=t[2]*n[0];
    P[3]+=t[0]*n[1];
    P[4]+=t[1]*n[1];
    P[5]+=t[2]*n[1];
    P[6]+=t[0]*n[2];
    P[7]+=t[1]*n[2];
    P[8]+=t[2]*n[2];

    K[0]=p[0];
    K[1]=0.;
    K[2]=p[1];
    K[3]=0.;
    K[4]=p[0];
    K[5]=p[2];
    K[6]=0.;
    K[7]=0.;
    K[8]=1.;
    iK[0]=1./p[0];
    iK[1]=0.;
    iK[2]=-p[1]/p[0];
    iK[3]=0.;
    iK[4]=1./p[0];
    iK[5]=-p[2]/p[0];
    iK[6]=0.;
    iK[7]=0.;
    iK[8]=1.;
    for(i=0;i<3;i++)
    {
        for(j=0;j<3;j++)
        {
             H[i*3+j]=K[i*3]*P[j*3];
             for(k=1;k<3;k++)
             {
                 H[i*3+j]+=K[i*3+k]*P[k+j*3];    
             }
        }
    }

    for(i=0;i<3;i++)
    {
        for(j=0;j<3;j++)
        {
             P[i+j*3]=H[i*3]*iK[j];
             for(k=1;k<3;k++)
             {
                 P[i+j*3]+=H[i*3+k]*iK[k*3+j];    
             }
        }
    }
}
